diff --git a/.github/workflows/build.yml b/.github/workflows/build.yml index 2575454280..e19ccef439 100755 --- a/.github/workflows/build.yml +++ b/.github/workflows/build.yml @@ -213,8 +213,6 @@ jobs: target: esp32s3 - path: 'components/rtps/example' target: esp32 - - path: 'components/rtps_embedded/example' - target: esp32 - path: 'components/rtsp/example' target: esp32 - path: 'components/runqueue/example' diff --git a/.github/workflows/rtps_interop.yml b/.github/workflows/rtps_interop.yml index 50795cc3e7..4252f2d22c 100644 --- a/.github/workflows/rtps_interop.yml +++ b/.github/workflows/rtps_interop.yml @@ -7,12 +7,11 @@ permissions: on: pull_request: paths: - - "components/rtps_embedded/**" - "components/rtps/**" - "components/socket/**" - "components/cdr/**" - "lib/espp.cmake" - - "pc/tests/rtps_embedded_*" + - "pc/tests/rtps_*" - ".github/workflows/rtps_interop.yml" workflow_dispatch: @@ -36,5 +35,5 @@ jobs: submodules: "recursive" - name: Run interop matrix run: | - cd components/rtps_embedded/interop + cd components/rtps/interop ./run.sh diff --git a/components/esp32-p4-function-ev-board/example/main/esp32_p4_function_ev_board_example.cpp b/components/esp32-p4-function-ev-board/example/main/esp32_p4_function_ev_board_example.cpp index 4389339c91..96afea8d0e 100644 --- a/components/esp32-p4-function-ev-board/example/main/esp32_p4_function_ev_board_example.cpp +++ b/components/esp32-p4-function-ev-board/example/main/esp32_p4_function_ev_board_example.cpp @@ -22,7 +22,7 @@ #include "cdr.hpp" #include "ping.hpp" -#include "rtps.hpp" +#include "rtps_participant.hpp" #include "esp32-p4-function-ev-board.hpp" @@ -31,11 +31,6 @@ using namespace std::chrono_literals; using Board = espp::Esp32P4FunctionEvBoard; -// Address of the most recently discovered RTPS peer (filled by the participant's -// on_participant_discovered callback, read by the ping self-test). -static std::mutex g_peer_mutex; -static std::string g_peer_addr; - static std::vector audio_bytes; static bool load_audio(size_t &out_size, size_t &out_sample_rate); @@ -363,10 +358,8 @@ extern "C" void app_main(void) { logger.info("BOOT button not initialized (shared with Ethernet RMII TXD1 pin)"); } - // Connectivity self-test: once we have an IP (and a moment for RTPS discovery), - // ping the gateway and the discovered peer once, then stop. This makes it easy - // to tell board-vs-network problems apart (e.g. gateway reachable but peer not - // => client isolation / L2 reachability problem, not the board). + // Connectivity self-test: once we have an IP, ping the gateway once, then stop. + // This makes it easy to tell board-vs-network problems apart. espp::Task ping_task( espp::Task::Config{.callback = [&logger](std::mutex &m, std::condition_variable &cv) -> bool { if (!have_ip) { @@ -374,7 +367,7 @@ extern "C" void app_main(void) { cv.wait_for(lk, 250ms); return false; // keep waiting for an IP } - // give RTPS discovery a few seconds to find the peer + // let the link settle for a few seconds before pinging { std::unique_lock lk(m); cv.wait_for(lk, 4s); @@ -387,16 +380,6 @@ extern "C" void app_main(void) { char gw[16] = {0}; esp_ip4addr_ntoa(&ip_info.gw, gw, sizeof(gw)); ping_target(logger, "gateway", gw); - std::string peer; - { - std::lock_guard lk(g_peer_mutex); - peer = g_peer_addr; - } - if (!peer.empty()) { - ping_target(logger, "peer", peer); - } else { - logger.warn("Ping self-test: no RTPS peer discovered yet to ping"); - } logger.info("=== Connectivity self-test done ==="); return true; // one-shot }, @@ -412,7 +395,6 @@ extern "C" void app_main(void) { const std::string topic = "espp/test/counter"; const std::string rtps_type = "std_msgs::msg::dds_::UInt32_"; uint32_t value = 0; - bool published = false; static constexpr auto loop_tick = 20ms; // RTPS loop tick static constexpr int64_t publish_period_us = 50'000'000; int64_t last_publish_us = 0; @@ -426,29 +408,19 @@ extern "C" void app_main(void) { std::string address = ip_str; logger.info("Got IP {}, starting RTPS participant", address); participant = std::make_shared(espp::RtpsParticipant::Config{ - .node_name = "espp_publisher", - .participant_id = 10, - .advertised_address = address, - .announce_period = 500ms, - .on_participant_discovered = - [&logger](const auto &p) { - { - std::lock_guard lk(g_peer_mutex); - if (g_peer_addr.empty()) { - g_peer_addr = p.address; - } - } - logger.info("discovered participant at {}", p.address); - }, - .on_endpoint_discovered = - [&logger](const auto &endpoint) { - logger.info("discovered {} '{}'", endpoint.is_reader ? "reader" : "writer", - endpoint.topic_name); + .interface_address = address, + // A remote reader (e.g. a ROS 2 subscriber) matched our writer: we now + // have a peer to publish to. The facade signals matches through this + // callback (it runs on an engine worker thread). + .on_publisher_matched = + [&logger]() { + rtps_has_peers = true; + logger.info("RTPS writer matched a remote reader"); }, .log_level = espp::Logger::Verbosity::DEBUG, }); participant->add_writer({ - .topic_name = topic, + .topic = topic, .type_name = rtps_type, }); if (!participant->start()) { @@ -460,6 +432,7 @@ extern "C" void app_main(void) { } else if (did_have_ip && !have_ip) { logger.warn("Lost IP, stopping RTPS participant"); participant.reset(); + rtps_has_peers = false; did_have_ip = false; } rtps_running = (participant != nullptr); @@ -467,11 +440,10 @@ extern "C" void app_main(void) { // Publish the next counter value at 2 Hz (independent of the status refresh). // Only publish if there is a discovered peer (otherwise the publish() call will return false). bool publish_period_elapsed = (now_us - last_publish_us) >= publish_period_us; - bool can_publish = - participant && !participant->discovered_participants().empty() && publish_period_elapsed; + bool can_publish = participant && rtps_has_peers.load() && publish_period_elapsed; if (can_publish) { last_publish_us = now_us; - published = participant->publish(topic, serialize_uint32(value)); + bool published = participant->publish(topic, serialize_uint32(value)); if (published) { logger.info("published {}", value); ++value; @@ -480,9 +452,9 @@ extern "C" void app_main(void) { } } - // Publish the RTPS counter/peer state for the status task to render. + // Publish the RTPS counter value for the status task to render (peer state + // is driven by on_publisher_matched above). rtps_value = value; - rtps_has_peers = published; // Stream any active playback to the speaker in chunks, advancing by // however much the stream buffer accepted diff --git a/components/rtps/CMakeLists.txt b/components/rtps/CMakeLists.txt index 1dc01861d6..c09058553a 100644 --- a/components/rtps/CMakeLists.txt +++ b/components/rtps/CMakeLists.txt @@ -1,4 +1,65 @@ idf_component_register( - INCLUDE_DIRS "include" - SRC_DIRS "src" - REQUIRES base_component cdr task socket) + SRCS + "src/rtps_participant.cpp" + "src/communication/EsppTransport.cpp" + "src/discovery/ParticipantProxyData.cpp" + "src/discovery/SEDPAgent.cpp" + "src/discovery/SPDPAgent.cpp" + "src/discovery/TopicData.cpp" + "src/entities/Domain.cpp" + "src/entities/Participant.cpp" + "src/entities/Reader.cpp" + "src/entities/StatefulReader.cpp" + "src/entities/StatefulWriter.cpp" + "src/entities/StatelessReader.cpp" + "src/entities/StatelessWriter.cpp" + "src/entities/Writer.cpp" + "src/messages/MessageReceiver.cpp" + "src/messages/MessageTypes.cpp" + "src/utils/Diagnostics.cpp" + INCLUDE_DIRS + "include" + REQUIRES + base_component cdr task thread_pool socket +) + +# Select the RTPS static-limits profile from Kconfig (see Kconfig in this +# component). The "embedded" profile is the default and is byte-identical to the +# historical behavior: it defines no RTPS_CONFIG_HEADER, so config.hpp selects +# rtps/config_esp32.hpp via ESP_PLATFORM. The relaxed "host" / "host_large" +# profiles override the profile header. These are capacity-only caps and do not +# change any bytes on the wire. +if(CONFIG_RTPS_LIMITS_PROFILE_HOST) + target_compile_definitions(${COMPONENT_LIB} PUBLIC RTPS_CONFIG_HEADER="rtps/config_desktop.hpp") +elseif(CONFIG_RTPS_LIMITS_PROFILE_HOST_LARGE) + target_compile_definitions(${COMPONENT_LIB} PUBLIC RTPS_CONFIG_HEADER="rtps/config_host_large.hpp") +endif() + +# Storage policy is orthogonal to the limits profile above: dynamic (heap, +# grow-on-full) storage is an explicit ESP opt-in (default static) so a relaxed +# 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. +if(CONFIG_RTPS_STORAGE_DYNAMIC) + target_compile_definitions(${COMPONENT_LIB} PUBLIC RTPS_STORAGE_DYNAMIC) +endif() + +# Best-effort DATA_FRAG fragmentation is opt-in on ESP targets (Kconfig, default +# off) so the MCU pays nothing for it by default. When enabled, define +# RTPS_ENABLE_FRAGMENTATION (compiles the fragment send/reassembly paths) and the +# reassembly cap RTPS_MAX_SAMPLE_SIZE (256 KB on the embedded profile). The +# facade's max payload size rises to this when fragmentation is enabled. +if(CONFIG_RTPS_ENABLE_FRAGMENTATION) + target_compile_definitions(${COMPONENT_LIB} PUBLIC + RTPS_ENABLE_FRAGMENTATION RTPS_MAX_SAMPLE_SIZE=262144) +endif() + +# The RPC layer (services + actions) is compiled in by default; the facade +# header defines RTPS_WITH_RPC unless RTPS_NO_RPC is set. When the Kconfig option +# is turned OFF, define RTPS_NO_RPC so the whole services/actions surface (and its +# std::thread/std::future use) is excluded, saving flash. ESP-only (this file is +# the ESP-IDF component build); host builds always keep RPC on. +if(NOT CONFIG_RTPS_ENABLE_RPC) + target_compile_definitions(${COMPONENT_LIB} PUBLIC RTPS_NO_RPC) +endif() + diff --git a/components/rtps_embedded/Kconfig b/components/rtps/Kconfig similarity index 96% rename from components/rtps_embedded/Kconfig rename to components/rtps/Kconfig index b5ed85b36c..d220ba0180 100644 --- a/components/rtps_embedded/Kconfig +++ b/components/rtps/Kconfig @@ -1,11 +1,11 @@ -menu "RTPS (rtps_embedded)" +menu "RTPS" choice RTPS_LIMITS_PROFILE prompt "RTPS static limits profile" default RTPS_LIMITS_PROFILE_EMBEDDED help Selects the compile-time capacity limits (profile header) used by the - rtps_embedded engine. Only the capacity caps differ between profiles. + rtps engine. Only the capacity caps differ between profiles. The storage MODEL (static vs dynamic) is a separate knob (RTPS_STORAGE_DYNAMIC below) and defaults to fully-static on ESP for every profile - selecting a relaxed profile does NOT enable heap diff --git a/components/rtps/README.md b/components/rtps/README.md index 4fe74aef86..278a7f0bf6 100644 --- a/components/rtps/README.md +++ b/components/rtps/README.md @@ -1,119 +1,201 @@ -# RTPS Component +# RTPS [![Badge](https://components.espressif.com/components/espp/rtps/badge.svg)](https://components.espressif.com/components/espp/rtps) -The `RtpsParticipant` component is the beginning of a cross-platform RTPS -(Real-Time Publish-Subscribe) implementation built on top of the ESPP `socket` -component. +ESPP component that integrates the [embeddedRTPS](https://github.com/embedded-software-laboratory/embeddedRTPS) +RTPS/DDS stack into the ESPP ecosystem, behind an idiomatic `espp::RtpsParticipant` +facade. Any platform that can build ESPP — ESP32, Linux, macOS, Windows — can use +it to interoperate with **ROS 2** nodes (rmw_fastrtps) or any DDS participant on +the network over the standard RTPS wire protocol. -This component now includes the first real RTPS discovery slice: +It provides three messaging patterns, all validated against live ROS 2 (Jazzy): -- RTPS header and DATA submessage framing helpers -- standard RTPS UDPv4 port calculations -- GUID, entity ID, locator, and sequence number utility types -- SPDP participant announcements using PL_CDR parameter lists -- SEDP publication and subscription announcements for local endpoints -- parsing and tracking of discovered remote participants, writers, and readers -- integration with the shared `cdr` component for CDR/PL_CDR payload handling -- optional best-effort user-data multicast transport, including endpoint-specific groups +- **Pub/sub** — topic-based, best-effort or reliable (HEARTBEAT/ACKNACK). +- **Services (RMI)** — request/reply with correlated responses. +- **Actions (AMI)** — long-running goals with feedback, result, and cancellation. -The long-term goal for this component is DDS/RTPS interoperability with ROS 2 -nodes, including best-effort and reliable user-data flows. Discovery is now -standards-shaped, but the reliable RTPS state machines (`HEARTBEAT`, -`ACKNACK`, resend windows) and ROS 2 endpoint/user-data interoperability are -still incomplete. +Each has a **typed** layer (reflectable structs, no manual bytes) and a +**byte-level** layer. Services and actions come in a ROS 2-interoperable flavour +and a lean **native** (espp ↔ espp) flavour. -## How RTPS Works +The upstream embeddedRTPS library hard-depends on FreeRTOS and lwIP; this +component removes those by routing all socket, task, and synchronisation through +ESPP's platform-agnostic `UdpSocket`, `Task`, `ThreadPool`, and `SocketReactor`. +On ESP32 those map to lwIP + FreeRTOS; elsewhere to the host OS. Micro-CDR is +gone — (de)serialization uses ESPP's reflection-driven `cdr`. -RTPS separates *metatraffic* from *user traffic*. +> **History:** this component was developed as `rtps_embedded` alongside an +> earlier from-scratch `rtps`; it is now the single `rtps` component. See +> [`REFACTOR_PLAN.md`](REFACTOR_PLAN.md) and [`RMI_AMI_DESIGN.md`](RMI_AMI_DESIGN.md). -- **Metatraffic** carries discovery and endpoint metadata. In this component, - that means SPDP participant announcements plus SEDP publication and - subscription announcements. -- **User traffic** carries application samples. Samples are sent as standard - RTPS `DATA` submessages whose `serializedPayload` is the raw CDR-encapsulated - sample (no ESPP-specific framing); the topic is identified by the writer GUID - resolved through SEDP discovery. The `publish()` / `on_sample` API works with - any CDR-encoded type. Reliable delivery (`HEARTBEAT`/`ACKNACK`) is not yet - implemented, so user traffic is currently best-effort. +--- -The current `RtpsParticipant` implementation opens three UDP sockets when -`start()` is called: +## Architecture -1. metatraffic multicast receive on the well-known SPDP multicast port -2. metatraffic unicast receive on the participant-specific discovery port -3. user unicast receive on the participant-specific user-data port - -It then starts a periodic announce task which multicasts SPDP and unicasts SEDP -endpoint announcements to each discovered peer. +The only platform-specific code is `EsppTransport`; everything above it is +portable C++23. The `espp::` facade is a thin, typed surface over the `rtps::` +engine. ```mermaid -flowchart LR - App["Application code"] --> Participant["RtpsParticipant"] - Participant --> SPDP["SPDP participant DATA"] - Participant --> SEDP["SEDP publication/subscription DATA"] - Participant --> User["User DATA submessages"] - SPDP --> MetaMC["Metatraffic multicast"] - SEDP --> MetaUC["Metatraffic unicast"] - User --> UserUC["User unicast"] - MetaMC --> Peer["Remote participant"] - MetaUC --> Peer - UserUC --> Peer +flowchart TD + U["User code / ROS 2 peer"] + + subgraph facade["espp:: facade (typed + byte-level)"] + direction TB + PS["Publisher / Subscriber (typed)"] + SVC["ServiceServer / ServiceClient"] + ACT["ActionServer / ActionClient"] + RP["espp::RtpsParticipant"] + PS --> RP + SVC --> RP + ACT --> RP + end + + subgraph engine["rtps:: engine (embeddedRTPS, de-vendored)"] + direction TB + DOM["rtps::Domain — packet routing + discovery"] + PART["rtps::Participant"] + WR["rtps::Writer (history + HEARTBEAT)"] + RD["rtps::Reader (ACKNACK + delivery)"] + DISC["SPDP + SEDP discovery agents"] + DOM --> PART --> WR & RD + DOM --> DISC + end + + subgraph plat["platform adapter (the ONLY porting layer)"] + direction TB + TR["rtps::EsppTransport"] + SOCK["espp::UdpSocket × N ports"] + REACT["espp::SocketReactor → espp::ThreadPool"] + CDR["espp::cdr (reflection CDR/XCDR)"] + TR --> SOCK --> REACT + end + + U --> facade --> engine --> plat + WR -. serialize .-> CDR + RD -. deserialize .-> CDR ``` -## Discovery Flow +| Build target | Socket backend | Task backend | +|---|---|---| +| ESP32 | lwIP (via ESP-IDF) | FreeRTOS | +| Linux / macOS / PC | POSIX sockets | `std::thread` | -At a high level, discovery proceeds like this: +Services and actions are **pure library code** over pub/sub — the only wire +addition is a `related_sample_identity` inline QoS on service request/reply (for +ROS 2 correlation). Actions add no wire primitive at all: they compose services +and topics. ```mermaid -sequenceDiagram - participant A as Local participant - participant MC as 239.255.0.1 - participant B as Remote participant - A->>MC: SPDP DATA(participant GUID, locators, enclave, builtin endpoints) - MC-->>B: multicast delivery - B->>MC: SPDP DATA(its participant metadata) - MC-->>A: multicast delivery - A->>B: SEDP publication DATA(topic/type/reliability) - A->>B: SEDP subscription DATA(topic/type/reliability) - B->>A: SEDP publication/subscription DATA - Note over A,B: Matching user-data traffic can then use the user-unicast ports +flowchart LR + subgraph patterns["Messaging patterns → RTPS primitives"] + direction TB + P1["Pub/sub"] --> W1["1 reliable/best-effort topic"] + P2["Service (RMI)"] --> W2["2 topics (rq/rr) + related_sample_identity"] + P3["Action (AMI)"] --> W3["3 services + 2 topics (feedback/status)"] + P4["Native service"] --> W4["2 es_rq/es_rr topics + 20-byte in-band header"] + P5["Native action"] --> W5["goal svc + cancel svc + 1 feedback topic (~4 endpoints)"] + end +``` + +--- + +## Quick-start (typed facade) + +```cpp +#include "rtps_participant.hpp" +#include "rtps_pubsub.hpp" // typed Publisher / Subscriber +#include "rtps_service.hpp" // typed ServiceServer / ServiceClient +#include "rtps_action.hpp" // typed ActionServer / ActionClient + +// Any reflectable struct is a message - fields map straight to CDR. +struct StringMsg { std::string data; }; +struct AddReq { int64_t a, b; }; +struct AddResp { int64_t sum; }; + +espp::RtpsParticipant participant({.interface_address = "192.168.1.10"}); +participant.start(); + +// Pub/sub +espp::Publisher pub(participant, {.topic = "rt/chatter", + .type_name = "std_msgs::msg::dds_::String_", + .reliability = espp::RtpsParticipant::Reliability::RELIABLE}); +espp::Subscriber sub(participant, {.topic = "rt/chatter", + .type_name = "std_msgs::msg::dds_::String_", + .on_message = [](const StringMsg &m) { /* use m.data */ }}); +pub.publish(StringMsg{"hello"}); + +// Service (RMI) - ros2 service call /add_two_ints ... hits this server +espp::ServiceServer server(participant, { + .service = "/add_two_ints", .type_name = "example_interfaces::srv::dds_::AddTwoInts", + .handler = [](const AddReq &r) { return AddResp{r.a + r.b}; }}); +espp::ServiceClient client(participant, { + .service = "/add_two_ints", .type_name = "example_interfaces::srv::dds_::AddTwoInts"}); +if (auto resp = client.call(AddReq{7, 35}, std::chrono::seconds(1))) { /* resp->sum == 42 */ } ``` -## Expected Compatibility - -The table below is intentionally conservative: **expected** means "this is the -intended scope based on the current wire format and code", not "fully verified -against every stack". - -| Peer implementation | Expected compatibility | Notes | -| --- | --- | --- | -| ESPP `rtps` component / `python/rtps_host.py` | **Yes** for current scaffold | Intended smoke-test path for SPDP, SEDP, and the standard CDR-over-RTPS best-effort user-data path. | -| Generic DDSI-RTPS 2.3 implementations | **Partial** | SPDP, SEDP, and best-effort `DATA` samples are standards-shaped; reliable delivery is not implemented today. | -| ROS 2 nodes backed by Fast DDS | **Partial / discovery-targeted** | Discovery includes ROS 2-relevant participant user data such as `enclave=...;`, and user samples are standard CDR-over-RTPS. Full ROS 2 topic interop additionally needs ROS 2 topic/type name mangling (`rt/...`, `std_msgs::msg::dds_::UInt32_`). | -| ROS 2 nodes backed by Cyclone DDS or other DDS vendors | **Partial / unverified** | Expected to be limited to the minimal discovery subset if the peer accepts the currently emitted parameter set; not validated yet. | -| Reliable DDS/RTPS endpoints | **No** | `HEARTBEAT`, `ACKNACK`, retransmission windows, and other reliable state-machine pieces are not implemented. | - -## Feature Status - -| Feature | Status | Notes | -| --- | --- | --- | -| RTPS header / DATA submessage serialize + parse | **Implemented** | Core message framing is present. | -| Standard UDPv4 RTPS port mapping | **Implemented** | Uses the DDSI-RTPS well-known port formula. | -| SPDP participant announce send/receive | **Implemented** | Multicast announce plus participant cache updates. | -| SEDP publication / subscription announce send/receive | **Implemented** | Local endpoints are announced and remote endpoints are cached. | -| Participant / endpoint discovery callbacks | **Implemented** | Exposed through `on_participant_discovered` and `on_endpoint_discovered`. | -| Standard CDR-over-RTPS user-data path | **Implemented** | `DATA` `serializedPayload` is the raw CDR sample; the `publish()` / `on_sample` API carries any CDR-encoded type, routed by writer GUID via SEDP. | -| Best-effort user-data multicast transport | **Implemented** | Supports shared participant-level multicast or endpoint-specific multicast locators advertised in SEDP; local readers only join the multicast groups configured for their topics. | -| QoS fields emitted in discovery | **Partial** | Reliability, durability, liveliness, and history parameters are advertised in SEDP. | -| QoS matching / policy enforcement | **Not implemented** | Remote QoS is parsed, but full writer/reader matching logic is still missing. | -| ROS 2 topic/type name mangling | **Not implemented** | Topic/type names are emitted verbatim; ROS 2 interop needs `rt/...` topic and `std_msgs::msg::dds_::UInt32_` type mangling. | -| Inline QoS handling | **Partial** | Inline QoS is skipped on receive to reach the payload; it is not emitted or interpreted. | -| Reliable RTPS (`HEARTBEAT`, `ACKNACK`, resend`) | **Not implemented** | Reliable delivery is not interoperable yet. | -| Full ROS 2 topic interoperability | **Not implemented** | Discovery is the current milestone; ROS 2-compatible data writers/readers are still pending. | +For ROS 2 interop use ROS 2 naming: topic `rt/`, type `::msg::dds_::_`. +The full request/reply + goal APIs (including the three client call styles and the +native protocol) are documented in +[`doc/en/protocols/rtps_rmi_ami.rst`](../../doc/en/protocols/rtps_rmi_ami.rst). + +### Byte-level API + +The typed wrappers are thin layers over `espp::RtpsParticipant`'s byte-level +methods (`add_writer`/`add_reader`/`publish`, `add_service_server`/`_client`, +`add_action_server`/`_client`, and the `add_native_*` variants), which take/return +CDR-encapsulated `std::span`. Use those for dynamic types. + +Python bindings expose the same surface via the `espp` module (see +[`python/rtps_rpc_demo.py`](../../python/rtps_rpc_demo.py)). + +--- + +## Configuration + +Capacity limits are chosen at build time by a **limits profile** header; storage +policy, fragmentation, and the RPC layer are separate, independent knobs. On +ESP32 these are ESP-IDF menuconfig options (under `RTPS`); on host they default +via `include/rtps/config.hpp`. + +| Knob | Options / default | Effect | +|---|---|---| +| `RTPS_LIMITS_PROFILE` | `embedded` (default) / `host` / `host_large` | Compile-time endpoint/history capacity caps (`config_esp32.hpp` / `config_desktop.hpp` / `config_host_large.hpp`). Wire-neutral. | +| `RTPS_STORAGE_DYNAMIC` | off on ESP32 / on host | Static `std::array` history (zero-heap, drop-oldest) vs heap-backed `std::deque` (grows). Orthogonal to the profile. | +| `RTPS_ENABLE_FRAGMENTATION` | off on ESP32 / on host | DATA_FRAG for samples > ~64 KB (interoperates with FastDDS/ROS 2). | +| `RTPS_ENABLE_RPC` | on (default) | Compile in services + actions (RMI/AMI). Disable to drop that code + its threads on a pure-pub/sub device. | + +The domain id, announcement/heartbeat periods, and pool sizes live in the profile +headers. + +--- + +## ESPP component dependencies + +| Component | Purpose | +|---|---| +| `base_component` | ESPP base class with integrated `espp::Logger` | +| `socket` | `UdpSocket` + `SocketReactor` used by `EsppTransport` | +| `task` | `espp::Task` / `espp::Timer` | +| `thread_pool` | shared worker pool for receive dispatch + async writer work | +| `cdr` | reflection-driven CDR/XCDR (de)serialization | + +The engine carries no vendored third-party code and has no direct dependency on +FreeRTOS, lwIP, or any platform library. + +--- ## Example -The [example](./example) exercises the protocol helpers, computes the standard -RTPS ports, builds/parses SPDP and SEDP messages, and demonstrates the -participant API without requiring a second device. +See [`example/`](example/) — an ESP32 (esp32-ethernet-kit) node that brings up a +participant over Ethernet and exercises the typed APIs: a `Publisher`/`Subscriber` +pair, a `ServiceServer` (`/add_two_ints`) + `ActionServer` (`/fibonacci`) a ROS 2 +client can drive, and a `ServiceClient` + `ActionClient`. A `menuconfig` option +adds a second, self-testing participant. See [`example/README.md`](example/README.md). + +## Interop & tests + +[`interop/`](interop/) runs a dockerised FastDDS / ROS 2 (Jazzy) matrix — golden +byte-for-byte wire tests, in-process loopbacks (pub/sub, services, actions, +native, typed), and live `ros2 service call` / `ros2 action send_goal` both +directions. It is gated in CI (`.github/workflows/rtps_interop.yml`). diff --git a/components/rtps_embedded/REFACTOR_PLAN.md b/components/rtps/REFACTOR_PLAN.md similarity index 100% rename from components/rtps_embedded/REFACTOR_PLAN.md rename to components/rtps/REFACTOR_PLAN.md diff --git a/components/rtps/RELIABLE_RTPS_PLAN.md b/components/rtps/RELIABLE_RTPS_PLAN.md deleted file mode 100644 index d4dcf48c1a..0000000000 --- a/components/rtps/RELIABLE_RTPS_PLAN.md +++ /dev/null @@ -1,192 +0,0 @@ -# Design: Reliable RTPS (HEARTBEAT / ACKNACK) for user data - -Status: **complete (pending final interop sign-off).** Phases 0–4 are -implemented and hardware-validated against Fast DDS/RTPS: - -- Phase 0 — submessage codecs (`SequenceNumberSet`, `HEARTBEAT`, `ACKNACK`, - `INFO_DST`, `GAP`) + dispatch seam. -- Phase 1 — reliable writer: history cache + HEARTBEAT emission. -- Phase 2 — reliable reader: dedup + in-order delivery + ACKNACK generation. -- Phase 3 — writer-side retransmission on ACKNACK, for **both** user-data - writers and the builtin **SEDP** writers, plus `GAP` for evicted/irrelevant - samples (emitted by the writer, honored by the reader). -- Phase 4 — hardening: builtin SEDP reliability (stable SEDP sequence numbers + - SEDP HEARTBEATs so a reliable peer recovers a missed announcement), per-reader - `DATA` `readerId` addressing, multi-homed multicast interface selection, and a - GUID-keyed `DiscoveryDb` for discovery state. - -Best-effort behavior is unchanged for endpoints that advertise `BEST_EFFORT`. -Reliable recovery now works in both directions: an espp reliable reader recovers -lost samples from a DDS/ROS 2 writer, and an espp reliable writer answers a -peer's NACKs (and GAPs samples it can no longer provide). - -## Goal - -Add interoperable `RELIABLE` delivery for user data: a stateful writer that -retains a history of sent samples and retransmits on request, and a stateful -reader that detects gaps and requests retransmission. The reliable handshake -runs only between endpoints that *both* advertise `RELIABLE` (discovered via -SEDP). DDS/ROS 2 interop is a first-class requirement. - -## Background — what exists today - -The user-data path is **stateless** (see `components/rtps/src/rtps.cpp`): - -- `publish()` (rtps.cpp:1150) builds one `DATA` submessage with a per-writer - monotonic sequence number (`next_user_data_sequence_number`) and sends it once - to each destination. There is no history cache and no retransmit. -- `handle_user_message()` (rtps.cpp:1427) parses `DATA`, resolves the topic via - the remote writer GUID, and invokes matching reader callbacks. For a reliable - writer it logs *"ACKNACK/HEARTBEAT is not implemented yet"* (1473) and delivers - anyway. There is no dedup, ordering, or per-writer receive state. -- `Message::parse` (rtps.cpp:793) decodes the generic submessage frame - (kind/flags/length/payload); only `DATA` is interpreted. `HEARTBEAT`/`ACKNACK` - exist in `SubmessageKind` (rtps.hpp:124) but are never parsed or generated. - `INFO_DST` is never emitted. - -What we can build on: - -- Per-writer monotonic sequence numbers and writer-GUID-based receive routing. -- `EndpointProxy` (rtps.hpp:202) already carries the remote `guid`, - `reliability`, `unicast_locator`, and `multicast_locators`, and - `ParticipantProxy` (rtps.hpp:191) carries the remote `address` + `ports`, so - the addressing data needed for stateful matching already exists in discovery. -- `parse_data_submessage` honors submessage endianness (the `E` flag) and skips - inline QoS — the new parsers mirror that. - -## Wire formats - -All submessages we emit are little-endian (`E` flag set), consistent with the -existing `DATA` path. Parsers honor the `E` flag for the submessage body. - -### SequenceNumber (8 bytes) -`high : int32` then `low : uint32` (each in submessage endianness). Already -handled by `ByteWriter::append_sequence_number_le` / `ByteReader::read_sequence_number`. - -### SequenceNumberSet -`bitmapBase : SequenceNumber (8)`, `numBits : uint32 (4)`, -`bitmap : uint32[ceil(numBits/32)]`. Bit *i* (MSB-first within each word) set ⇒ -`bitmapBase + i` is requested/missing. `numBits` is 0..256. This is the fiddliest -piece — unit-tested in isolation (empty set, single bit, 256-bit max, base -alignment, word boundaries). - -### HEARTBEAT (0x07) -Body: `readerId(4) writerId(4) firstSN(8) lastSN(8) count : uint32(4)`. -Flags: `E` (endian, bit 0), `F` (final — no response required, bit 1), -`L` (liveliness, bit 2). `firstSN..lastSN` is the inclusive range of sequence -numbers currently available in the writer's history. `count` is monotonic per -writer (stale-heartbeat detection). - -### ACKNACK (0x06) -Body: `readerId(4) writerId(4) readerSNState : SequenceNumberSet count : uint32(4)`. -Flags: `E`, `F` (final). `readerSNState.bitmapBase` is the lowest sequence number -the reader still needs; the bitmap marks which of `[base, base+numBits)` are -missing. An empty set with `base = lastSN + 1` is a positive ack of everything. -`count` is monotonic per reader. - -### INFO_DST (0x0e) -Body: `guidPrefix(12)`. Prefixes a directed HEARTBEAT/ACKNACK so the receiving -participant routes it to the right entity. Required for robust DDS interop. - -## Architecture - -### Writer side (`WriterReliableState`, per local reliable writer) -- `history : std::map>` — CDR payloads keyed by SN, - capped to a KEEP_LAST depth (drop oldest, advance `first_sn`). -- `first_sn`, `last_sn`, `heartbeat_count`. -- Per matched reliable reader: highest acked SN (`ReaderProxy`), to know when a - sample can be purged and whether a heartbeat still needs a response. -- On `publish()`: store sample → send `DATA` → send a (non-final) `HEARTBEAT` - (INFO_DST + HEARTBEAT) to each matched reliable reader's unicast locator. -- Periodic `HEARTBEAT` (extend `announce_task_` or a dedicated heartbeat task, - with jitter) while any reader has unacked samples. -- On `ACKNACK`: resend the requested SNs still in history to the requesting - reader's locator; advance that reader's acked watermark (`base - 1`). - -### Reader side (`ReaderReliableState`, per (local reader, remote writer GUID)) -- Highest-contiguous received SN + a bounded out-of-order/reorder set, - `last_heartbeat_count`, `acknack_count`. -- On `DATA` from a reliable writer: record the SN, **dedup**, deliver in SN order - (bounded reorder buffer). -- On `HEARTBEAT`: compute missing SNs in `[firstSN, lastSN]`, reply with `ACKNACK` - (INFO_DST + ACKNACK) addressed to the writer's user-unicast endpoint (resolved - from `discovered_writers_ → participant_guid → ParticipantProxy.address/ports.user_unicast`, - **not** the raw `Socket::Info` source port). Respond to non-final heartbeats - even when caught up so the writer can stop repeating / purge. - -### Addressing -- Writer → reader (HEARTBEAT, DATA resend): `EndpointProxy.unicast_locator` of the - matched remote reader. -- Reader → writer (ACKNACK): the writer's participant user-unicast address+port. -- Both are sent on the existing `user_unicast_receiver_` socket. Phase 1–3 are - unicast-only; reliable-over-multicast is a later extension. - -### Concurrency -The history cache and the reader/writer proxy maps are touched by the publish -path (app thread), the receive path (one or more receive tasks), and the -heartbeat/retransmit timer. Guard them with a dedicated `reliable_mutex_` -(distinct from `mutex_` / `receivers_mutex_` / `sequence_mutex_`); never hold it -nested under `mutex_`. Document the lock ordering as: `mutex_` (discovery state) -is taken to snapshot endpoints, released, then `reliable_mutex_` for reliable -state — matching the existing snapshot-then-act pattern in -`build_user_send_configs`. - -### Config additions -- `WriterConfig.history_depth` (KEEP_LAST, default e.g. 16); replaces the - `kHistoryKeepLast = 0` placeholder (rtps.cpp:54) and feeds the SEDP history QoS. -- `Config.heartbeat_period` (default ~200 ms) and `Config.reliable_reorder_depth`. - -## Phase plan - -- **Phase 0 — submessage codecs + dispatch seam** *(this PR)*. Add the - `SequenceNumberSet` codec, `HEARTBEAT`/`ACKNACK`/`INFO_DST` build + parse - helpers (internal, endian-aware), and dispatch `HEARTBEAT`/`ACKNACK` in - `handle_user_message` to `handle_heartbeat_submessage` / `handle_acknack_submessage` - handlers (initially logging at debug). No behavior change for best-effort; the - reliable downgrade warning stays until Phase 2. Round-trip tests for the codecs. -- **Phase 1 — reliable writer** *(done)*: `WriterReliableState` history cache - (keyed by SN, bounded by `WriterConfig.history_depth`), sample cached on - `publish()`, and HEARTBEAT (INFO_DST + HEARTBEAT) emitted after publish and - periodically (`Config.heartbeat_period`, `heartbeat_task_`) to each matched - reliable reader's unicast locator. SEDP now advertises the real history depth. - Guarded by `reliable_mutex_`. The ACKNACK-driven retransmission response is - Phase 3. -- **Phase 2 — reliable reader** *(done)*: `ReaderReliableState` keyed by - "#"; `deliver_reliable_sample()` dedups and delivers - in order via a bounded reorder buffer (`Config.reliable_reorder_depth`); - `send_acknack_for_heartbeat()` replies to a HEARTBEAT with an `INFO_DST + - ACKNACK` carrying the `SequenceNumberSet` of missing SNs (or a positive ack), - addressed to the writer's unicast locator (fallback: participant user-unicast). - Handles writer-purged gaps (advance past lost SNs) and stale heartbeats. - Reliable handshake runs only when both endpoints advertise RELIABLE; the - downgrade warning is removed. -- **Phase 3 — writer retransmission** *(done)*: on ACKNACK, a writer resends the - NACKed SNs still in history to the requesting reader. Shared - `build_directed_data_message` (INFO_DST + DATA addressed to the requesting - reader) drives both `retransmit_user_data` (from the per-writer history) and - `retransmit_sedp` (rebuilt deterministically from `writers_`/`readers_` via the - stable index-based SEDP sequence numbers — no separate SEDP cache). Samples no - longer in history are answered with a `GAP`; the reader's `apply_gap` advances - its frontier over the irrelevant SNs and releases anything buffered behind them. -- **Phase 4 — hardening** *(done)*: builtin SEDP reliability (stable SEDP SNs + - SEDP HEARTBEATs so a reliable peer ACKNACKs and we retransmit a missed - announcement), per-reader `DATA` `readerId` addressing, `stop()` cleanup of all - reliable state, and bounded reorder/irrelevant buffers. (Remaining nice-to-haves: - initial heartbeat on reader match, stale-count rejection, jittered timers.) -- **Phase 5 — interop validation** *(in progress)*: hardware-validated publishing - from an ESP32-P4 (Ethernet) to a Fast RTPS subscriber, including the SEDP - HEARTBEAT/ACKNACK exchange. Still TODO: induced packet-loss tests and Cyclone. - -## Design decisions -- **History model**: KEEP_LAST depth per writer (bounded memory on embedded), not - KEEP_ALL, by default. -- **Delivery**: dedup + monotonic in-order with a small bounded reorder buffer. -- **INFO_DST**: emitted from the start (needed for DDS interop). -- **Endianness**: emit little-endian; parse honoring the `E` flag. -- **Scope**: unicast reliable first. - -## Out of scope (later) -- Durability beyond VOLATILE (TRANSIENT_LOCAL replay to late joiners). -- Fragmentation (`DATA_FRAG`) for samples larger than the MTU. -- Full QoS-incompatibility reporting. -- Reliable-over-multicast (the reliable handshake is unicast-only). diff --git a/components/rtps_embedded/RMI_AMI_DESIGN.md b/components/rtps/RMI_AMI_DESIGN.md similarity index 100% rename from components/rtps_embedded/RMI_AMI_DESIGN.md rename to components/rtps/RMI_AMI_DESIGN.md diff --git a/components/rtps/example/CMakeLists.txt b/components/rtps/example/CMakeLists.txt index e64b1c087f..81e617e013 100644 --- a/components/rtps/example/CMakeLists.txt +++ b/components/rtps/example/CMakeLists.txt @@ -11,7 +11,7 @@ set(EXTRA_COMPONENT_DIRS set( COMPONENTS - "main esptool_py logger rtps wifi" + "main esptool_py logger cdr timer rtps esp32-ethernet-kit" CACHE STRING "List of components to include" ) diff --git a/components/rtps/example/README.md b/components/rtps/example/README.md index 0c2183c98a..66decd4005 100644 --- a/components/rtps/example/README.md +++ b/components/rtps/example/README.md @@ -1,77 +1,67 @@ # RTPS Example -This example now acts as a two-node RTPS smoke test for ESP targets on the same -Wi-Fi network. +This example brings up an `espp::RtpsParticipant` on an **ESP32-Ethernet-Kit** and +demonstrates every typed API the `rtps` component offers, interoperable +with FastDDS / ROS 2 over the standard RTPS wire protocol. It demonstrates: -* Wi-Fi STA setup for host-network RTPS traffic -* standard RTPS UDP port calculation for each participant -* SPDP participant discovery between two boards -* SEDP endpoint discovery for request/response topics -* CDR little-endian serialization for `std_msgs/msg/UInt32`-style payloads -* best-effort inter-node request/response sample exchange - -The component's long-term goal is ROS 2 interoperability over DDS/RTPS. This -example focuses on proving cross-board discovery and user-data delivery using -the current scaffold. +- Ethernet bring-up (DHCP **server** on `192.168.4.1/24`, so a directly-attached + PC gets an address) and starting a participant on the interface's IPv4 address +- a typed `Publisher` / `Subscriber` pair (reliable pub/sub) + that pairs with the FastDDS host peer in [`pc/host_pubsub.cpp`](pc/host_pubsub.cpp) +- a typed **service server** (`/add_two_ints`) and **action server** + (`/fibonacci`) the device hosts — a ROS 2 client can drive them directly with + `ros2 service call` / `ros2 action send_goal` (no manual CDR; reflectable + `AddReq`/`AddResp`, `FibGoal`/`FibSeq` structs) +- a typed **service client** + **action client** that call a peer's + `/peer_add_two_ints` / `/peer_fib` (run a ROS 2 / rclpy server for those names + to see a full round-trip; otherwise the calls simply time out, still exercising + the client API) + +All of the RMI/AMI code is compiled out when `RTPS_ENABLE_RPC` is disabled. ## How to use example -### Configure two boards - -Build one board as the **initiator** and the other as the **responder**. - -For both boards: - -1. Set the same `RTPS domain ID`, `Topic prefix`, `WiFi SSID`, and `WiFi password`. -2. Give each board a unique `RTPS participant ID`. -3. Give each board a distinct `Participant node name` or keep the role-specific defaults. -4. If you want to exercise multicast user data, enable `Use best-effort user-data multicast` - on both boards and keep the same request/response multicast groups on each node. - -Fresh example configurations now default to: +### Configure -* initiator: participant ID `1`, node name `espp_rtps_initiator` -* responder: participant ID `2`, node name `espp_rtps_responder` - -If you are reusing an older build directory or `sdkconfig`, rerun `idf.py menuconfig` -or delete the stale generated config so the old shared defaults (`participant ID = 1`, -`node name = espp_rtps_node`) do not persist on both boards. - -For one board only: +```bash +idf.py menuconfig +``` -1. Select `RTPS Example Configuration -> Example Role -> Initiator` +Under **RTPS Example Configuration**: -For the other board: +| Option | Description | +|---|---| +| `RTPS_EXAMPLE_ANNOUNCE_PERIOD_MS` | Period of the outgoing publisher (default 1500 ms). | +| `RTPS_EXAMPLE_SECOND_PARTICIPANT` | Additively bring up a second, self-testing participant that calls the device's own `/add_two_ints` + `/fibonacci` for a full on-device round-trip (default off; roughly doubles the RTPS engine RAM). | -1. Select `RTPS Example Configuration -> Example Role -> Responder` +Under **RTPS** you can also toggle the limits profile, dynamic +storage, DATA_FRAG fragmentation, and the RPC (services + actions) layer. ### Build and Flash -Build the project and flash it to the board, then run monitor tool to view -serial output: - -```sh +```bash idf.py -p PORT flash monitor ``` -(Replace PORT with the name of the serial port to use.) - -## Example Output +Replace `PORT` with the serial port. Connect the board's Ethernet port to a PC +(or a switch) so the participant has a network. -The initiator waits until it discovers the responder's endpoints, then publishes -incrementing values on `/request`. The responder logs each -received request and echoes the same value back on `/response`. +### Talk to it -Expected signs of success: +- **Pub/sub host peer** (FastDDS): build and run [`pc/host_pubsub.cpp`](pc/) + against the board's topics. +- **ROS 2**: with `example_interfaces` installed and on the same network/domain: + ```bash + ros2 service call /add_two_ints example_interfaces/srv/AddTwoInts "{a: 7, b: 35}" + ros2 action send_goal -f /fibonacci example_interfaces/action/Fibonacci "{order: 5}" + ``` -* both boards report Wi-Fi connection and their local IP address -* both boards log RTPS participant and endpoint discovery -* the initiator logs `Published request N` followed by `Received response N` -* the responder logs `Received request N, sending response` +## Expected Output -When multicast user data is enabled, discovery still uses the normal RTPS -metatraffic sockets, but request samples are published to the configured request -multicast group and response samples are published to the configured response -multicast group. Each node only joins the group for the topic it subscribes to. +The monitor logs Ethernet link-up + the assigned IP, then `tx`/`rx` lines for the +publisher/subscriber and `service '/add_two_ints' + action '/fibonacci' ready`. +Each ROS 2 call logs the handled request/goal (e.g. `service add_two_ints: 7 + 35 += 42`, `action fibonacci(5) done`). With the second participant enabled, `[self-test]` +lines report `PASS`/`FAIL` for the local round-trips. diff --git a/components/rtps/example/main/CMakeLists.txt b/components/rtps/example/main/CMakeLists.txt index e398cc2f07..7fcdc059fa 100644 --- a/components/rtps/example/main/CMakeLists.txt +++ b/components/rtps/example/main/CMakeLists.txt @@ -1,3 +1,4 @@ idf_component_register(SRC_DIRS "." INCLUDE_DIRS "." - REQUIRES logger rtps wifi) + REQUIRES cdr timer rtps esp32-ethernet-kit logger + ) diff --git a/components/rtps/example/main/Kconfig.projbuild b/components/rtps/example/main/Kconfig.projbuild index 7d4a64dbcf..1843ad2716 100644 --- a/components/rtps/example/main/Kconfig.projbuild +++ b/components/rtps/example/main/Kconfig.projbuild @@ -1,109 +1,25 @@ menu "RTPS Example Configuration" - choice RTPS_EXAMPLE_ROLE - prompt "Example Role" - default RTPS_EXAMPLE_ROLE_INITIATOR - help - Build one board as the initiator and a second board as the responder - to verify RTPS discovery and end-to-end UInt32 sample exchange. - - config RTPS_EXAMPLE_ROLE_INITIATOR - bool "Initiator" - help - Publishes incrementing request samples after it discovers a - responder, then logs the matching responses. - - config RTPS_EXAMPLE_ROLE_RESPONDER - bool "Responder" - help - Waits for request samples and echoes the same value back on the - response topic. - - endchoice - - config RTPS_EXAMPLE_NODE_NAME - string "Participant node name" - default "espp_rtps_initiator" if RTPS_EXAMPLE_ROLE_INITIATOR - default "espp_rtps_responder" if RTPS_EXAMPLE_ROLE_RESPONDER - help - Logical RTPS participant name announced during discovery. - - config RTPS_EXAMPLE_DOMAIN_ID - int "RTPS domain ID" - range 0 231 - default 0 - help - Both boards must use the same domain ID to discover each other. - - config RTPS_EXAMPLE_PARTICIPANT_ID - int "RTPS participant ID" - range 0 119 - default 1 if RTPS_EXAMPLE_ROLE_INITIATOR - default 2 if RTPS_EXAMPLE_ROLE_RESPONDER - help - Each board should use a unique participant ID within the same domain. - - config RTPS_EXAMPLE_TOPIC_PREFIX - string "Topic prefix" - default "espp/rtps_example" - help - Prefix used to derive the request and response topics. - config RTPS_EXAMPLE_ANNOUNCE_PERIOD_MS - int "Discovery announce period (ms)" - range 200 10000 + int "Publish period (ms)" + range 200 60000 default 1500 help - Period between periodic SPDP/SEDP discovery announcements. + Period between outgoing messages published by the MCU. - config RTPS_EXAMPLE_PUBLISH_PERIOD_MS - int "Initiator publish period (ms)" - range 250 60000 - default 2000 - depends on RTPS_EXAMPLE_ROLE_INITIATOR - help - Period between request messages sent by the initiator after a - responder has been discovered. - - config RTPS_EXAMPLE_USE_USER_MULTICAST - bool "Use best-effort user-data multicast" + config RTPS_EXAMPLE_SECOND_PARTICIPANT + bool "Add a second (self-test) participant that calls the local servers" default n - help - If enabled, the example advertises and uses topic-specific - multicast groups for the request and response topics instead of - per-participant unicast delivery. - - config RTPS_EXAMPLE_REQUEST_MULTICAST_GROUP - string "Request topic multicast group" - default "239.255.0.11" - depends on RTPS_EXAMPLE_USE_USER_MULTICAST - help - IPv4 multicast group used for the /request topic. - - config RTPS_EXAMPLE_RESPONSE_MULTICAST_GROUP - string "Response topic multicast group" - default "239.255.0.12" - depends on RTPS_EXAMPLE_USE_USER_MULTICAST - help - IPv4 multicast group used for the /response topic. - - config ESP_WIFI_SSID - string "WiFi SSID" - default "" - help - SSID (network name) for the example to connect to. - - config ESP_WIFI_PASSWORD - string "WiFi Password" - default "" - help - WiFi password (WPA or WPA2) for the example to use. - - config ESP_MAXIMUM_RETRY - int "Maximum retry" - default 5 - help - Set the maximum retry count to avoid reconnecting forever when the - network is unavailable. + depends on RTPS_ENABLE_RPC + help + Additively bring up a SECOND RtpsParticipant on the device, with its + own typed ServiceClient + ActionClient, that call THIS device's own + /add_two_ints service and /fibonacci action. A participant filters out + its own messages, so this is the only way to fully round-trip the + client APIs on one device with no external peer - a self-contained + self-test. Off by default: a second participant roughly doubles the + RTPS engine's RAM (two discovery stacks, socket sets, and pools), which + may not fit on a plain ESP32 without PSRAM. Independent of the + peer-facing client demo, which always runs. endmenu diff --git a/components/rtps/example/main/rtps_example.cpp b/components/rtps/example/main/rtps_example.cpp index 57859b6f29..d306d49b1c 100644 --- a/components/rtps/example/main/rtps_example.cpp +++ b/components/rtps/example/main/rtps_example.cpp @@ -1,286 +1,283 @@ -#include +// rtps component example (ESP32 / esp32-ethernet-kit). +// +// Brings up an espp::RtpsParticipant over Ethernet and demonstrates the typed +// APIs: a Publisher/Subscriber pair (pub/sub), typed ServiceServer + +// ActionServer the device hosts, and typed ServiceClient + ActionClient the +// device runs. See components/rtps/example/README.md and the RMI/AMI +// docs (doc/en/protocols/rtps_rmi_ami.rst). + #include #include -#include -#include -#include #include -#include -#include "cdr.hpp" +#include "esp32-ethernet-kit.hpp" + #include "logger.hpp" -#include "rtps.hpp" -#include "wifi_sta.hpp" +#include "rtps_action.hpp" +#include "rtps_participant.hpp" +#include "rtps_pubsub.hpp" +#include "rtps_service.hpp" +#include "timer.hpp" using namespace std::chrono_literals; -namespace { -constexpr std::string_view kTypeName = "std_msgs/msg/UInt32"; - -// Thin wrappers over the `cdr` component for the std_msgs/msg/UInt32 payload used by this example. -// The rtps `publish()` / `on_sample` API works with any CDR-encoded type, so serialization lives in -// the application rather than the participant. -std::vector serialize_uint32(uint32_t value) { - // XCDR1 = classic little-endian CDR with a 4-byte encapsulation header — the on-the-wire - // format ROS 2 uses for std_msgs/msg/UInt32. - auto bytes = cdr::serialize(value); - if (!bytes) { - return {}; - } - const auto *data = reinterpret_cast(bytes->data()); - return std::vector(data, data + bytes->size()); -} - -std::optional deserialize_uint32(std::span cdr_payload) { - // deserialize() reads the encapsulation header itself (version + endianness). - auto value = cdr::deserialize(std::as_bytes(cdr_payload)); - if (!value) { - return std::nullopt; - } - return *value; -} - -bool run_local_protocol_checks(espp::Logger &logger, const espp::RtpsParticipant &participant) { - auto announce_message = participant.build_announce_message(); - auto parsed_message = espp::RtpsParticipant::Message::parse(announce_message); - if (!parsed_message) { - logger.error("Failed to parse locally built announce message"); - return false; - } - logger.info("Built and parsed SPDP announce message with {} submessage(s)", - parsed_message->submessages.size()); - - if (!participant.writers().empty()) { - auto sedp_publication_message = - participant.build_sedp_publication_message(participant.writers().front()); - auto parsed_publication_message = - espp::RtpsParticipant::Message::parse(sedp_publication_message); - if (!parsed_publication_message) { - logger.error("Failed to parse locally built SEDP publication message"); - return false; - } - logger.info("Built and parsed SEDP publication message with {} submessage(s)", - parsed_publication_message->submessages.size()); - } - - if (!participant.readers().empty()) { - auto sedp_subscription_message = - participant.build_sedp_subscription_message(participant.readers().front()); - auto parsed_subscription_message = - espp::RtpsParticipant::Message::parse(sedp_subscription_message); - if (!parsed_subscription_message) { - logger.error("Failed to parse locally built SEDP subscription message"); - return false; - } - logger.info("Built and parsed SEDP subscription message with {} submessage(s)", - parsed_subscription_message->submessages.size()); - } +// std_msgs/msg/String as a plain reflectable struct. The typed Publisher / +// Subscriber serialize any such struct to the DDS wire format (ROS 2 / classic +// CDR) with no manual (de)serialization in application code. +struct StringMsg { + std::string data; +}; - auto uint32_payload = serialize_uint32(42); - auto maybe_value = deserialize_uint32(uint32_payload); - if (!maybe_value || *maybe_value != 42) { - logger.error("UInt32 CDR round trip failed"); - return false; - } - logger.info("UInt32 CDR round trip succeeded with value {}", *maybe_value); - return true; -} - -[[maybe_unused]] bool has_endpoint(std::span endpoints, - std::string_view topic_name, bool is_reader) { - return std::any_of(endpoints.begin(), endpoints.end(), - [topic_name, is_reader](const auto &endpoint) { - return endpoint.topic_name == topic_name && endpoint.is_reader == is_reader; - }); -} -} // namespace +// Reflectable request/reply + goal/result structs for the typed service + action +// servers below. Their fields map straight to CDR, matching example_interfaces +// so a ROS 2 client (ros2 service call / ros2 action send_goal) can drive them. +struct AddReq { + int64_t a; + int64_t b; +}; +struct AddResp { + int64_t sum; +}; +struct FibGoal { + int32_t order; +}; +struct FibSeq { + std::vector sequence; +}; extern "C" void app_main(void) { espp::Logger logger({.tag = "rtps_example", .level = espp::Logger::Verbosity::INFO}); //! [rtps example] - std::string ip_address; - espp::WifiSta wifi_sta({.ssid = CONFIG_ESP_WIFI_SSID, - .password = CONFIG_ESP_WIFI_PASSWORD, - .num_connect_retries = CONFIG_ESP_MAXIMUM_RETRY, - .on_connected = nullptr, - .on_disconnected = nullptr, - .on_got_ip = [&ip_address](ip_event_got_ip_t *eventdata) { - ip_address = fmt::format("{}.{}.{}.{}", IP2STR(&eventdata->ip_info.ip)); - fmt::print("got IP: {}\n", ip_address); - }}); - - logger.info("Waiting for WiFi connection..."); - while (!wifi_sta.is_connected()) { + // Bring up Ethernet (DHCP server on 192.168.4.1/24 so a directly-attached PC + // gets an address); any espp network interface works - the RTPS participant + // only needs the interface's IPv4 address. + auto &board = espp::Esp32EthernetKit::get(); + bool eth_ok = board.initialize_ethernet({ + .mode = espp::Esp32EthernetKit::DhcpMode::SERVER, + .on_link_up = [&]() { logger.info("Ethernet link up"); }, + .on_link_down = [&]() { logger.warn("Ethernet link down"); }, + }); + if (!eth_ok) { + logger.error("Ethernet initialization failed"); + return; + } + logger.info("Waiting for Ethernet link..."); + while (!board.is_ethernet_connected()) { std::this_thread::sleep_for(100ms); } - logger.info("WiFi connected, local IP {}", ip_address); - - const std::string node_name = CONFIG_RTPS_EXAMPLE_NODE_NAME; - const std::string topic_prefix = CONFIG_RTPS_EXAMPLE_TOPIC_PREFIX; - const std::string request_topic = topic_prefix + "/request"; - const std::string response_topic = topic_prefix + "/response"; -#if CONFIG_RTPS_EXAMPLE_USE_USER_MULTICAST -#if defined(CONFIG_RTPS_EXAMPLE_REQUEST_MULTICAST_GROUP) && \ - defined(CONFIG_RTPS_EXAMPLE_RESPONSE_MULTICAST_GROUP) - const std::string request_multicast_group = CONFIG_RTPS_EXAMPLE_REQUEST_MULTICAST_GROUP; - const std::string response_multicast_group = CONFIG_RTPS_EXAMPLE_RESPONSE_MULTICAST_GROUP; -#elif defined(CONFIG_RTPS_EXAMPLE_USER_MULTICAST_GROUP) - const std::string request_multicast_group = CONFIG_RTPS_EXAMPLE_USER_MULTICAST_GROUP; - const std::string response_multicast_group = CONFIG_RTPS_EXAMPLE_USER_MULTICAST_GROUP; -#else - const std::string request_multicast_group = "239.255.0.11"; - const std::string response_multicast_group = "239.255.0.12"; -#endif -#else - const std::string request_multicast_group; - const std::string response_multicast_group; -#endif + auto eth_ip = board.ethernet_ip(); + const std::string interface_address = + fmt::format("{}.{}.{}.{}", esp_ip4_addr1_16(ð_ip), esp_ip4_addr2_16(ð_ip), + esp_ip4_addr3_16(ð_ip), esp_ip4_addr4_16(ð_ip)); + logger.info("Ethernet up, IP {}", interface_address); - std::atomic request_count{0}; - std::atomic response_count{0}; - std::atomic next_request_value{1}; - std::atomic last_sent_request{0}; + // RTPS/DDS participant (embeddedRTPS engine behind the espp facade). The + // topics pair with the FastDDS host peer in example/pc/host_pubsub.cpp; for + // ROS 2 instead, use topic "rt/" with type "::msg::dds_::_" + // (e.g. "rt/chatter" + "std_msgs::msg::dds_::String_"). + constexpr const char *pub_topic = "mcu_to_pc"; + constexpr const char *sub_topic = "pc_to_mcu"; + constexpr const char *type_name = "std_msgs::msg::String"; + // Automatic locals: they RAII-clean up in reverse order on any early return + // (subscriber/publisher stop referencing the participant before it is + // destroyed), and the trailing while(true) keeps them alive in normal use. espp::RtpsParticipant participant({ - .node_name = node_name, - .domain_id = CONFIG_RTPS_EXAMPLE_DOMAIN_ID, - .participant_id = CONFIG_RTPS_EXAMPLE_PARTICIPANT_ID, - .advertised_address = ip_address, - .announce_period = std::chrono::milliseconds(CONFIG_RTPS_EXAMPLE_ANNOUNCE_PERIOD_MS), - .on_participant_discovered = - [&logger](const auto &proxy) { - logger.info("Discovered participant '{}' at {} (meta {}, user {})", - proxy.name.empty() ? proxy.guid_prefix.to_string() : proxy.name, - proxy.address, proxy.ports.metatraffic_unicast, proxy.ports.user_unicast); - }, - .on_endpoint_discovered = - [&logger](const auto &endpoint) { - logger.info("Discovered remote {} '{}' [{}]", endpoint.is_reader ? "reader" : "writer", - endpoint.topic_name, endpoint.type_name); - }, + .interface_address = interface_address, + .on_publisher_matched = [&]() { logger.info("publisher matched a remote reader"); }, + .on_subscriber_matched = [&]() { logger.info("subscriber matched a remote writer"); }, .log_level = espp::Logger::Verbosity::INFO, - // Keep the underlying UDP sockets quieter than the participant so routine socket activity - // does not clutter the logs. Raise this to debug transport issues independently. - .socket_log_level = espp::Logger::Verbosity::WARN, }); + if (!participant.start()) { + logger.error("Failed to start the RTPS participant"); + return; + } -#if CONFIG_RTPS_EXAMPLE_ROLE_INITIATOR - participant.add_writer({ - .topic_name = request_topic, - .type_name = std::string(kTypeName), - .reliability = espp::RtpsParticipant::ReliabilityKind::BEST_EFFORT, - .multicast_group = request_multicast_group, - .entity_index = 0, - }); - participant.add_reader({ - .topic_name = response_topic, - .type_name = std::string(kTypeName), - .reliability = espp::RtpsParticipant::ReliabilityKind::BEST_EFFORT, - .multicast_group = response_multicast_group, - .entity_index = 0, - .on_sample = - [&logger, &response_count, &last_sent_request](std::span cdr) { - auto value = deserialize_uint32(cdr); - if (!value) { - return; + // Typed reliable publisher: publish StringMsg structs directly (HEARTBEAT/ + // ACKNACK-acknowledged, retransmitted to matched readers). No manual CDR. + using Reliability = espp::RtpsParticipant::Reliability; + espp::Publisher publisher(participant, { + .topic = pub_topic, + .type_name = type_name, + .reliability = Reliability::RELIABLE, + }); + // Typed subscriber: receive StringMsg structs directly. + espp::Subscriber subscriber( + participant, { + .topic = sub_topic, + .type_name = type_name, + .on_message = [&](const StringMsg &msg) { logger.info("rx: {}", msg.data); }, + }); + if (!publisher.is_valid() || !subscriber.is_valid()) { + logger.error("Failed to create the typed publisher/subscriber"); + return; + } + + // Publish a counter periodically via the typed publisher. + uint32_t counter = 0; + espp::Timer publish_timer({ + .name = "rtps_pub", + .period = std::chrono::milliseconds(CONFIG_RTPS_EXAMPLE_ANNOUNCE_PERIOD_MS), + .callback = + [&]() { + if (publisher.publish(StringMsg{fmt::format("msg {}", counter++)})) { + logger.info("tx: msg {}", counter - 1); + } else { + logger.warn("tx dropped (history full)"); } - response_count++; - logger.info("Received response {} (expected {})", *value, last_sent_request.load()); + return false; // keep the timer running }, + .log_level = espp::Logger::Verbosity::WARN, }); -#else - auto *participant_ptr = &participant; - participant.add_writer({ - .topic_name = response_topic, - .type_name = std::string(kTypeName), - .reliability = espp::RtpsParticipant::ReliabilityKind::BEST_EFFORT, - .multicast_group = response_multicast_group, - .entity_index = 0, - }); - participant.add_reader({ - .topic_name = request_topic, - .type_name = std::string(kTypeName), - .reliability = espp::RtpsParticipant::ReliabilityKind::BEST_EFFORT, - .multicast_group = request_multicast_group, - .entity_index = 0, - .on_sample = - [&logger, &request_count, &response_topic, - &participant_ptr](std::span cdr) { - auto value = deserialize_uint32(cdr); - if (!value) { - return; + logger.info("started: pub='{}' sub='{}' type='{}'", pub_topic, sub_topic, type_name); + +#ifdef RTPS_WITH_RPC + // Typed service (RMI) server: a ROS 2 client can `ros2 service call + // /add_two_ints example_interfaces/srv/AddTwoInts "{a: 7, b: 35}"` and get 42. + // No manual CDR - the reflectable AddReq/AddResp structs are (de)serialized for + // us. (Compiled out when CONFIG_RTPS_ENABLE_RPC is disabled.) + espp::ServiceServer add_service( + participant, { + .service = "/add_two_ints", + .type_name = "example_interfaces::srv::dds_::AddTwoInts", + .handler = + [&](const AddReq &r) { + logger.info("service add_two_ints: {} + {} = {}", r.a, r.b, r.a + r.b); + return AddResp{r.a + r.b}; + }, + }); + + // Typed action (AMI) server: a ROS 2 client can `ros2 action send_goal + // /fibonacci example_interfaces/action/Fibonacci "{order: 5}"` and receive + // feedback + the [0,1,1,2,3,5] result. execute() runs on its own thread. + espp::ActionServer fib_action( + participant, { + .action = "/fibonacci", + .type_name = "example_interfaces::action::dds_::Fibonacci", + .on_goal = [&](const FibGoal &g) { return g.order > 0; }, + .execute = + [&](auto &h) { + const int32_t order = h.goal().order; + std::vector seq{0, 1}; + for (int32_t i = 1; i < order; ++i) { + seq.push_back(seq[i] + seq[i - 1]); + h.publish_feedback(FibSeq{seq}); + std::this_thread::sleep_for(200ms); + } + h.succeed(FibSeq{seq}); + logger.info("action fibonacci({}) done", order); + }, + }); + if (!add_service.is_valid() || !fib_action.is_valid()) { + logger.error("Failed to create the typed service/action servers"); + return; + } + logger.info("service '/add_two_ints' + action '/fibonacci' ready"); + + // Also demonstrate the CLIENT side on-device: a typed service client + action + // client that call services a peer hosts ("/peer_add_two_ints", "/peer_fib"). + // Run a ROS 2 / rclpy server (or another espp device) for those names to see a + // full round-trip; until then the calls simply time out (logged), which still + // exercises the client API on-target. (Calling this device's OWN services is + // not possible - a participant filters out its own messages.) + espp::ServiceClient add_client( + participant, + {.service = "/peer_add_two_ints", .type_name = "example_interfaces::srv::dds_::AddTwoInts"}); + espp::ActionClient fib_client( + participant, + {.action = "/peer_fib", .type_name = "example_interfaces::action::dds_::Fibonacci"}); + + // Only one action goal in flight at a time: without a peer the goal never + // completes, so re-sending on every tick would leak a pending goal each time. + // The service call() below self-cleans on its 1s timeout, so it can run freely. + std::atomic fib_in_flight{false}; + espp::Timer rpc_client_timer({ + .name = "rtps_rpc_client", + .period = 5s, + .callback = + [&]() { + // Typed blocking service call (RMI). + if (auto resp = add_client.call(AddReq{20, 22}, 1s)) { + logger.info("[client] /peer_add_two_ints(20,22) = {}", resp->sum); + } else { + logger.info("[client] /peer_add_two_ints: no reply (peer serving it?)"); } - request_count++; - logger.info("Received request {}, sending response", *value); - if (!participant_ptr->publish(response_topic, serialize_uint32(*value))) { - logger.warn("Failed to publish response {}", *value); + // Typed action goal (AMI) with typed feedback + result. Skip if the + // previous goal has not finished (e.g. no peer is serving it). + if (!fib_in_flight.exchange(true)) { + fib_client.send_goal( + FibGoal{5}, [&](const FibSeq &) { /* per-feedback */ }, + [&](espp::GoalStatus status, const FibSeq &res) { + logger.info("[client] /peer_fib result: status={} len={}", + static_cast(status), res.sequence.size()); + fib_in_flight.store(false); + }); } + return false; // keep the timer running }, + .log_level = espp::Logger::Verbosity::WARN, }); -#endif - - auto ports = participant.ports(); - logger.info("Role: {}", -#if CONFIG_RTPS_EXAMPLE_ROLE_INITIATOR - "initiator" -#else - "responder" -#endif - ); - logger.info("Participant name: {}", node_name); - logger.info("Participant GUID: {}", participant.participant_guid().to_string()); - logger.info("Domain ID: {}, Participant ID: {}", CONFIG_RTPS_EXAMPLE_DOMAIN_ID, - CONFIG_RTPS_EXAMPLE_PARTICIPANT_ID); - logger.info("Topic prefix: {}", topic_prefix); - logger.info("Request topic: {}, Response topic: {}", request_topic, response_topic); - logger.info("Ports: meta mc={}, meta uc={}, user mc={}, user uc={}", ports.metatraffic_multicast, - ports.metatraffic_unicast, ports.user_multicast, ports.user_unicast); -#if CONFIG_RTPS_EXAMPLE_USE_USER_MULTICAST - logger.info("User-data multicast enabled: request group {}, response group {}", - request_multicast_group, response_multicast_group); -#endif - - if (!run_local_protocol_checks(logger, participant)) { + if (!add_client.is_valid() || !fib_client.is_valid()) { + logger.error("Failed to create the typed service/action clients"); return; } + logger.info("client for '/peer_add_two_ints' + '/peer_fib' running"); - if (!participant.start()) { - logger.error("Failed to start RTPS participant"); +#if CONFIG_RTPS_EXAMPLE_SECOND_PARTICIPANT + // Purely additive on-device SELF-TEST (Kconfig, default off): a SECOND + // participant with its own service + action clients that call THIS device's own + // /add_two_ints and /fibonacci servers, for a full local round-trip (a + // participant filters out its own messages, so the loopback needs a distinct + // participant). This roughly doubles the RTPS engine RAM - only enable on a + // target with headroom (e.g. PSRAM). + espp::RtpsParticipant selftest_participant({ + .interface_address = interface_address, + .log_level = espp::Logger::Verbosity::WARN, + }); + if (!selftest_participant.start()) { + logger.error("Failed to start the self-test participant"); + return; + } + espp::ServiceClient selftest_add_client( + selftest_participant, + {.service = "/add_two_ints", .type_name = "example_interfaces::srv::dds_::AddTwoInts"}); + espp::ActionClient selftest_fib_client( + selftest_participant, + {.action = "/fibonacci", .type_name = "example_interfaces::action::dds_::Fibonacci"}); + espp::Timer selftest_timer({ + .name = "rtps_selftest", + .period = 5s, + .callback = + [&]() { + if (auto resp = selftest_add_client.call(AddReq{20, 22}, 2s)) { + logger.info("[self-test] /add_two_ints(20,22) = {} ({})", resp->sum, + resp->sum == 42 ? "PASS" : "FAIL"); + } else { + logger.warn("[self-test] /add_two_ints: no reply"); + } + selftest_fib_client.send_goal( + FibGoal{5}, [&](const FibSeq &) {}, + [&](espp::GoalStatus status, const FibSeq &res) { + const std::vector expected{0, 1, 1, 2, 3, 5}; + const bool ok = status == espp::GoalStatus::SUCCEEDED && res.sequence == expected; + logger.info("[self-test] /fibonacci(5) len={} ({})", res.sequence.size(), + ok ? "PASS" : "FAIL"); + }); + return false; // keep the timer running + }, + .log_level = espp::Logger::Verbosity::WARN, + }); + if (!selftest_add_client.is_valid() || !selftest_fib_client.is_valid()) { + logger.error("Failed to create the self-test clients"); return; } + logger.info("self-test participant round-tripping the local service + action"); +#endif // CONFIG_RTPS_EXAMPLE_SECOND_PARTICIPANT +#endif // RTPS_WITH_RPC //! [rtps example] -#if CONFIG_RTPS_EXAMPLE_ROLE_INITIATOR - logger.info("Initiator is waiting for a responder on the same domain/topic prefix..."); - while (true) { - auto remote_readers = participant.discovered_readers(); - auto remote_writers = participant.discovered_writers(); - bool request_reader_ready = has_endpoint(remote_readers, request_topic, true); - bool response_writer_ready = has_endpoint(remote_writers, response_topic, false); - if (!request_reader_ready || !response_writer_ready) { - logger.info("Waiting for responder endpoints (request_reader={}, response_writer={})", - request_reader_ready, response_writer_ready); - std::this_thread::sleep_for(2s); - continue; - } - - auto value = next_request_value.fetch_add(1); - last_sent_request = value; - if (participant.publish(request_topic, serialize_uint32(value))) { - logger.info("Published request {} on '{}'", value, request_topic); - } else { - logger.warn("Failed to publish request {}", value); - } - std::this_thread::sleep_for(std::chrono::milliseconds(CONFIG_RTPS_EXAMPLE_PUBLISH_PERIOD_MS)); - } -#else - logger.info("Responder is ready and will echo '{}' samples back on '{}'", request_topic, - response_topic); while (true) { - std::this_thread::sleep_for(5s); - logger.info("Responder status: discovered participants={}, requests handled={}", - participant.discovered_participants().size(), request_count.load()); + std::this_thread::sleep_for(1s); } -#endif } diff --git a/components/rtps_embedded/example/pc/CMakeLists.txt b/components/rtps/example/pc/CMakeLists.txt similarity index 85% rename from components/rtps_embedded/example/pc/CMakeLists.txt rename to components/rtps/example/pc/CMakeLists.txt index 9e94541939..e15ebb46f2 100644 --- a/components/rtps_embedded/example/pc/CMakeLists.txt +++ b/components/rtps/example/pc/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.20) -project(rtps_embedded_host_pc_example LANGUAGES CXX) +project(rtps_host_pc_example LANGUAGES CXX) find_package(fastdds REQUIRED COMPONENTS shared) find_package(OpenSSL REQUIRED) diff --git a/components/rtps_embedded/example/pc/host_pubsub.cpp b/components/rtps/example/pc/host_pubsub.cpp similarity index 100% rename from components/rtps_embedded/example/pc/host_pubsub.cpp rename to components/rtps/example/pc/host_pubsub.cpp diff --git a/components/rtps/example/sdkconfig.defaults b/components/rtps/example/sdkconfig.defaults index e823df850d..7952923ade 100644 --- a/components/rtps/example/sdkconfig.defaults +++ b/components/rtps/example/sdkconfig.defaults @@ -1,8 +1,23 @@ +CONFIG_COMPILER_OPTIMIZATION_PERF=y + +# Set the FreeRTOS rate to 1ms (1000Hz) +CONFIG_FREERTOS_HZ=1000 + +CONFIG_ESP_DEFAULT_CPU_FREQ_MHZ_240=y +CONFIG_ESP_DEFAULT_CPU_FREQ_MHZ=240 + # Common ESP-related # CONFIG_ESP_SYSTEM_EVENT_TASK_STACK_SIZE=4096 CONFIG_ESP_MAIN_TASK_STACK_SIZE=8192 +# +# Ethernet (ESP32-Ethernet-Kit A V1.2 — IP101GRI RMII PHY) +# +CONFIG_ETH_ENABLED=y +CONFIG_ETH_USE_ESP32_EMAC=y +CONFIG_ETH_PHY_USE_IP101=y + # # Partition Table # diff --git a/components/rtps/idf_component.yml b/components/rtps/idf_component.yml index 03ce02ae4b..b9a5018e8e 100644 --- a/components/rtps/idf_component.yml +++ b/components/rtps/idf_component.yml @@ -1,26 +1,25 @@ ## IDF Component Manager Manifest File license: "MIT" -description: "Cross-platform RTPS protocol foundation component for ESP-IDF" +description: "RTPS / DDS component for ESP-IDF: ROS 2-interoperable pub/sub, services (RMI), and actions (AMI) over an embeddedRTPS engine with espp transport and task abstractions" +documentation: "https://esp-cpp.github.io/espp/protocols/rtps.html" url: "https://github.com/esp-cpp/espp/tree/main/components/rtps" repository: "git://github.com/esp-cpp/espp.git" maintainers: - - William Emfinger -documentation: "https://esp-cpp.github.io/espp/protocols/rtps.html" + - William Emfinger + - Liyun Guo examples: - path: example tags: - cpp - - Component - RTPS - DDS - - ROS2 - - UDP - - Discovery - - PubSub + - networking + - embedded dependencies: idf: version: '>=5.0' espp/base_component: '>=1.0' espp/cdr: '>=1.0' - espp/socket: '>=1.0' espp/task: '>=1.0' + espp/thread_pool: '>=1.0' + espp/socket: '>=1.0' diff --git a/components/rtps/include/rtps.hpp b/components/rtps/include/rtps.hpp deleted file mode 100644 index fa36f3bc42..0000000000 --- a/components/rtps/include/rtps.hpp +++ /dev/null @@ -1,576 +0,0 @@ -#pragma once - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include "base_component.hpp" -#include "task.hpp" -#include "udp_socket.hpp" - -namespace espp { -/// Cross-platform RTPS protocol foundation built on top of the socket component. -/// -/// \section rtps_ex1 RTPS Example -/// \snippet rtps_example.cpp rtps example -class RtpsParticipant : public BaseComponent { -public: - /// @brief Delivery semantics advertised for a writer or reader endpoint. - enum class ReliabilityKind : uint8_t { - BEST_EFFORT = 0, ///< Best-effort delivery semantics. - RELIABLE = 1, ///< Reliable delivery semantics. - }; - - /// @brief RTPS protocol version carried in the RTPS message header. - struct ProtocolVersion { - uint8_t major{2}; ///< Major RTPS version number. - uint8_t minor{3}; ///< Minor RTPS version number. - }; - - /// @brief RTPS vendor identifier carried in the RTPS message header. - struct VendorId { - std::array value{0xca, 0xfe}; ///< Two-byte vendor identifier. - }; - - /// @brief 12-byte prefix that identifies an RTPS participant. - struct GuidPrefix { - std::array value{}; ///< Raw 12-byte GUID prefix value. - - /// @brief Compare two GUID prefixes for equality. - /// @param other Prefix to compare against. - /// @return True if both prefixes contain identical bytes. - bool operator==(const GuidPrefix &other) const = default; - - /// @brief Convert the GUID prefix to a printable hex string. - /// @return Colon-separated hexadecimal representation of the prefix. - std::string to_string() const; - }; - - /// @brief 4-byte entity identifier within a participant. - struct EntityId { - std::array value{}; ///< Raw 4-byte entity identifier value. - - /// @brief Compare two entity identifiers for equality. - /// @param other Entity identifier to compare against. - /// @return True if both entity identifiers contain identical bytes. - bool operator==(const EntityId &other) const = default; - - /// @brief Convert the entity identifier to a printable hex string. - /// @return Colon-separated hexadecimal representation of the entity identifier. - std::string to_string() const; - }; - - /// @brief Globally unique identifier for an RTPS entity. - struct Guid { - GuidPrefix prefix{}; ///< Participant GUID prefix portion. - EntityId entity_id{}; ///< Entity identifier portion within the participant. - - /// @brief Compare two GUIDs for equality. - /// @param other GUID to compare against. - /// @return True if both GUIDs have the same prefix and entity identifier. - bool operator==(const Guid &other) const = default; - - /// @brief Convert the GUID to a printable string. - /// @return Combined printable representation of the prefix and entity identifier. - std::string to_string() const; - }; - - /// @brief RTPS sequence number wrapper. - struct SequenceNumber { - int64_t value{1}; ///< Signed 64-bit RTPS sequence number value. - }; - - /// @brief RTPS network locator for a unicast or multicast transport endpoint. - struct Locator { - /// @brief Supported locator transport kinds. - enum class Kind : int32_t { - INVALID = -1, ///< Locator is not initialized or not valid. - UDP_V4 = 1, ///< UDP over IPv4 locator. - }; - - Kind kind{Kind::INVALID}; ///< Transport kind of this locator. - uint32_t port{0}; ///< Transport port number in host byte order. - std::array address{}; ///< Raw 16-byte RTPS locator address field. - - /// @brief Build a UDPv4 locator from a dotted IPv4 address and port. - /// @param ipv4_address Dotted-decimal IPv4 address string. - /// @param port UDP port number to advertise. - /// @return A locator configured for UDPv4 with the IPv4 address stored in RTPS locator form. - static Locator udp_v4(std::string_view ipv4_address, uint16_t port); - - /// @brief Convert the locator address to a printable IPv4 string. - /// @return Dotted-decimal IPv4 address, or 0.0.0.0 if the locator is not UDPv4. - std::string address_string() const; - }; - - /// @brief RTPS message header fields. - struct Header { - ProtocolVersion protocol_version{}; ///< RTPS protocol version. - VendorId vendor_id{}; ///< Sender vendor identifier. - GuidPrefix guid_prefix{}; ///< Sender participant GUID prefix. - }; - - /// @brief Supported RTPS submessage kinds used by the current implementation. - enum class SubmessageKind : uint8_t { - PAD = 0x01, ///< Padding submessage. - ACKNACK = 0x06, ///< Reliable-reader acknowledgement submessage. - HEARTBEAT = 0x07, ///< Reliable-writer heartbeat submessage. - GAP = 0x08, ///< Writer notification that sequence numbers are irrelevant/unavailable. - INFO_TS = 0x09, ///< Timestamp information submessage. - INFO_DST = 0x0e, ///< Destination GUID-prefix information submessage. - DATA = 0x15, ///< User or discovery data submessage. - }; - - /// @brief One RTPS submessage within an RTPS message. - struct Submessage { - SubmessageKind kind{SubmessageKind::PAD}; ///< Submessage kind discriminator. - uint8_t flags{0x01}; ///< Raw RTPS submessage flags byte. - std::vector - payload{}; ///< Serialized submessage payload bytes without the 4-byte submessage header. - }; - - /// @brief RTPS message consisting of a header and a sequence of submessages. - struct Message { - Header header{}; ///< RTPS message header. - std::vector submessages{}; ///< Serialized submessages carried by this RTPS message. - - /// @brief Serialize the RTPS message to bytes. - /// @return A complete RTPS message buffer ready to send on the network. - std::vector serialize() const; - - /// @brief Parse an RTPS message from bytes. - /// @param data Serialized RTPS message bytes. - /// @return A parsed message on success, or std::nullopt if the input is invalid. - static std::optional parse(std::span data); - }; - - /// @brief Standard RTPS UDP port mapping derived from domain and participant IDs. - struct PortMapping { - uint16_t metatraffic_multicast{0}; ///< Multicast discovery/metatraffic port. - uint16_t metatraffic_unicast{0}; ///< Unicast discovery/metatraffic port for this participant. - uint16_t user_multicast{0}; ///< Multicast user-data port. - uint16_t user_unicast{0}; ///< Unicast user-data port for this participant. - }; - - /// @brief Configuration for a locally advertised writer endpoint. - struct WriterConfig { - std::string topic_name{}; ///< Topic name advertised through SEDP. - std::string type_name{"std_msgs/msg/UInt32"}; ///< Type name advertised through SEDP. - ReliabilityKind reliability{ - ReliabilityKind::BEST_EFFORT}; ///< Reliability QoS advertised for the writer. - std::string multicast_group{}; ///< Optional multicast group advertised for this writer and used - ///< by publish() when set. - uint32_t entity_index{0}; ///< Local entity slot used to derive the RTPS entity ID. - uint32_t history_depth{16}; ///< KEEP_LAST history depth advertised through SEDP and used to - ///< bound the reliable-writer history cache (number of recent - ///< samples retained for retransmission). Only used when - ///< reliability is RELIABLE. - }; - - /// @brief Configuration for a locally advertised reader endpoint. - struct ReaderConfig { - std::string topic_name{}; ///< Topic name advertised through SEDP. - std::string type_name{"std_msgs/msg/UInt32"}; ///< Type name advertised through SEDP. - ReliabilityKind reliability{ - ReliabilityKind::BEST_EFFORT}; ///< Reliability QoS advertised for the reader. - std::string multicast_group{}; ///< Optional multicast group advertised for this reader and - ///< joined on the standard RTPS user-multicast port when set. - uint32_t entity_index{0}; ///< Local entity slot used to derive the RTPS entity ID. - std::function)> on_sample{ - nullptr}; ///< Callback invoked with the raw CDR-encapsulated serialized payload - ///< (encapsulation header + body) of a matching received sample. The span is only - ///< valid for the duration of the callback. - }; - - /// @brief Cached information about a discovered remote participant. - struct ParticipantProxy { - Guid participant_guid{}; ///< Discovered participant GUID. - GuidPrefix guid_prefix{}; ///< Discovered participant GUID prefix. - std::string name{}; ///< Remote participant name, if advertised. - std::string enclave{"/"}; ///< Remote ROS 2 enclave/user-data hint, if advertised. - std::string address{}; ///< Preferred remote IPv4 address for user traffic. - PortMapping ports{}; ///< Remote participant port mapping derived from discovery data. - uint32_t builtin_endpoints{0}; ///< Remote builtin-endpoint bitmask from SPDP. - Locator metatraffic_unicast_locator{}; ///< Full metatraffic unicast locator (address + port). - Locator metatraffic_multicast_locator{}; ///< Full metatraffic multicast locator. - Locator default_unicast_locator{}; ///< Full default (user-data) unicast locator. - Locator default_multicast_locator{}; ///< Full default (user-data) multicast locator. - }; - - /// @brief Cached information about a discovered remote reader or writer endpoint. - struct EndpointProxy { - Guid guid{}; ///< Discovered endpoint GUID. - Guid participant_guid{}; ///< GUID of the participant that owns this endpoint. - std::string topic_name{}; ///< Discovered topic name. - std::string type_name{"std_msgs/msg/UInt32"}; ///< Discovered type name. - ReliabilityKind reliability{ReliabilityKind::BEST_EFFORT}; ///< Advertised endpoint reliability. - bool is_reader{false}; ///< True for discovered readers, false for discovered writers. - bool expects_inline_qos{false}; ///< Whether the remote endpoint requested inline QoS. - Locator unicast_locator{}; ///< Preferred unicast locator advertised by the endpoint. - std::vector multicast_locators{}; ///< Multicast locators advertised by the endpoint - ///< for user-data traffic. - }; - - /// @brief Top-level participant configuration. - struct Config { - std::string node_name{"espp_rtps"}; ///< Local participant name advertised in discovery. - uint16_t domain_id{0}; ///< RTPS domain ID used for port derivation and discovery scope. - uint16_t participant_id{0}; ///< RTPS participant ID used for GUID and port derivation. - bool randomize_guid_prefix{true}; ///< When true (default), mix per-instance entropy into the - ///< participant GUID so a restarted participant is seen as a - ///< new participant - DDS/ROS 2 peers then accept its - ///< republished samples instead of dropping them as - ///< already-seen duplicates. Set false for a deterministic - ///< GUID derived only from node_name/domain/participant id - ///< (e.g. reproducible tests). - std::string bind_address{"0.0.0.0"}; ///< Local IPv4 address to bind sockets to. - std::string advertised_address{ - "127.0.0.1"}; ///< IPv4 address advertised to peers for unicast traffic. - std::string metatraffic_multicast_group{ - "239.255.0.1"}; ///< Multicast group used for RTPS metatraffic discovery. - std::string user_multicast_group{ - "239.255.0.1"}; ///< Multicast group used for best-effort user-data multicast when enabled. - bool use_multicast_for_user_data{false}; ///< If true, join the user multicast group and publish - ///< temporary user-data samples via multicast. - Task::BaseConfig receive_task_config{ - .name = "RtpsRx", - .stack_size_bytes = 6 * 1024}; ///< Base task configuration for receive sockets. - Task::BaseConfig announce_task_config{ - .name = "RtpsAnnounce", - .stack_size_bytes = 6 * 1024}; ///< Task configuration for periodic discovery announcements. - std::chrono::milliseconds announce_period{1000}; ///< Interval between periodic SPDP/SEDP sends. - Task::BaseConfig heartbeat_task_config{ - .name = "RtpsHeartbeat", .stack_size_bytes = 6 * 1024}; ///< Task configuration for periodic - ///< reliable-writer heartbeats. - std::chrono::milliseconds heartbeat_period{200}; ///< Interval between periodic HEARTBEATs sent - ///< for reliable writers with cached samples. - uint32_t reliable_reorder_depth{ - 32}; ///< Max number of out-of-order samples buffered per reliable reader for in-order - ///< delivery. Samples beyond this are dropped and re-requested via ACKNACK. - std::string enclave{"/"}; ///< User-data enclave string advertised in SPDP. - std::function on_participant_discovered{ - nullptr}; ///< Callback invoked when a remote participant is first discovered. - std::function on_endpoint_discovered{ - nullptr}; ///< Callback invoked when a remote endpoint is first discovered. - espp::Logger::Verbosity log_level{ - espp::Logger::Verbosity::INFO}; ///< Participant log verbosity. - espp::Logger::Verbosity socket_log_level{ - espp::Logger::Verbosity::WARN}; ///< Log verbosity for the participant's underlying UDP - ///< sockets. Defaults to WARN so routine socket activity - ///< does not clutter the logs; raise it to debug transport - ///< issues independently of the participant log level. - }; - - /// @brief Construct an RTPS participant. - /// @param config Participant configuration controlling ports, addresses, discovery, and - /// callbacks. - explicit RtpsParticipant(const Config &config); - - /// @brief Destroy the participant and stop any active sockets/tasks. - ~RtpsParticipant(); - - /// @brief Start discovery sockets, user-data sockets, and periodic announcements. - /// @return True if startup succeeded, false if the participant was already started or - /// initialization failed. - bool start(); - - /// @brief Stop sockets and background tasks associated with the participant. - void stop(); - - /// @brief Check whether the participant is currently started. - /// @return True if the participant has been started and not yet stopped. - bool is_started() const; - - /// @brief Register a local writer endpoint to advertise through SEDP. - /// @param writer_config Writer configuration to add. - /// @return True after the writer has been stored. - bool add_writer(const WriterConfig &writer_config); - - /// @brief Register a local reader endpoint to advertise through SEDP. - /// @param reader_config Reader configuration to add. - /// @return True after the reader has been stored. - bool add_reader(const ReaderConfig &reader_config); - - /// @brief Get the currently discovered remote participants. - /// @return A snapshot copy of the discovered participant list. - std::vector discovered_participants() const; - - /// @brief Get the currently discovered remote writer endpoints. - /// @return A snapshot copy of the discovered writer list. - std::vector discovered_writers() const; - - /// @brief Get the currently discovered remote reader endpoints. - /// @return A snapshot copy of the discovered reader list. - std::vector discovered_readers() const; - - /// @brief Access the registered local writer configurations. - /// @return A snapshot copy of the local writer list. - std::vector writers() const; - - /// @brief Access the registered local reader configurations. - /// @return A snapshot copy of the local reader list. - std::vector readers() const; - - /// @brief Compute the standard RTPS UDP port mapping for this participant. - /// @return The derived metatraffic and user-data ports for the configured domain and participant - /// IDs. - PortMapping ports() const; - - /// @brief Get the local participant GUID. - /// @return GUID built from the local GUID prefix and participant entity ID. - Guid participant_guid() const; - - /// @brief Get the GUID for a local writer entity slot. - /// @param index Zero-based local writer entity index. - /// @return GUID for the derived local writer entity. - Guid writer_guid(size_t index) const; - - /// @brief Get the GUID for a local reader entity slot. - /// @param index Zero-based local reader entity index. - /// @return GUID for the derived local reader entity. - Guid reader_guid(size_t index) const; - - /// @brief Build the default participant announce message. - /// @return Serialized SPDP participant announce message. - std::vector build_announce_message() const; - - /// @brief Build the SPDP participant announcement message for this participant. - /// @return Serialized SPDP message describing the local participant. - std::vector build_spdp_announce_message() const; - - /// @brief Build an SEDP publication announcement for a local writer. - /// @param writer_config Writer configuration to serialize. - /// @return Serialized SEDP publication message for the writer. - std::vector build_sedp_publication_message(const WriterConfig &writer_config) const; - - /// @brief Build an SEDP subscription announcement for a local reader. - /// @param reader_config Reader configuration to serialize. - /// @return Serialized SEDP subscription message for the reader. - std::vector build_sedp_subscription_message(const ReaderConfig &reader_config) const; - - /// @brief Build a standard RTPS user-data DATA message carrying a CDR-encoded sample. - /// @param writer_config Local writer configuration used for the topic and writer entity ID. - /// @param cdr_payload CDR-encapsulated serialized payload (encapsulation header + body) to carry - /// as the DATA submessage serializedPayload. - /// @return Serialized RTPS DATA message containing the sample. - std::vector build_data_message(const WriterConfig &writer_config, - std::span cdr_payload) const; - - /// @brief Publish a CDR-encoded sample on a topic using the configured user-data transport. - /// @param topic_name Topic name to publish on. Must match a registered local writer. - /// @param cdr_payload CDR-encapsulated serialized payload (encapsulation header + body) to send. - /// @return True if at least one send call succeeded, false otherwise. - bool publish(std::string_view topic_name, std::span cdr_payload); - - /// @brief Compute the standard RTPS UDP port mapping for a domain/participant pair. - /// @param domain_id RTPS domain ID. - /// @param participant_id RTPS participant ID. - /// @return Derived RTPS metatraffic and user-data ports. - static PortMapping compute_port_mapping(uint16_t domain_id, uint16_t participant_id); - -private: - struct UserMulticastReceiver { - std::string multicast_group{}; - std::unique_ptr socket{}; - }; - - bool handle_metatraffic_message(std::vector &data, const Socket::Info &sender); - bool handle_user_message(std::vector &data, const Socket::Info &sender); - bool ensure_user_multicast_receivers_started(const std::string &extra_group = {}); - - /// @brief A user-data send destination plus the RTPS reader entity it targets. - /// @details For unicast sends to a specific matched reader, reader_id is that - /// reader's entity id so the DATA submessage addresses it directly; - /// for multicast sends (which target all matched readers) it is left - /// as ENTITYID_UNKNOWN. - struct UserDataDestination { - UdpSocket::SendConfig send_config{}; - EntityId reader_id{}; - }; - std::vector build_user_send_configs(std::string_view topic_name, - const WriterConfig &writer_config) const; - int64_t next_spdp_sequence_number() const; - int64_t next_sedp_publication_sequence_number() const; - int64_t next_sedp_subscription_sequence_number() const; - int64_t next_user_data_sequence_number(uint32_t entity_index) const; - bool send_spdp_announce_now(); - bool send_sedp_announcements_to(const ParticipantProxy &participant); - bool send_discovery_now(); - - // SEDP serialized-payload builders (the parameter list of a publication/subscription sample, - // without the DATA submessage framing) so the same sample can be re-sent under a stable sequence - // number and retransmitted on ACKNACK. - std::vector build_sedp_publication_payload(const WriterConfig &writer_config) const; - std::vector build_sedp_subscription_payload(const ReaderConfig &reader_config) const; - // Build an INFO_DST + DATA message directed at a specific reader (used for both user-data and - // SEDP retransmission). - std::vector - build_directed_data_message(const GuidPrefix &dest_prefix, EntityId reader_id, EntityId writer_id, - int64_t sequence_number, - std::span serialized_payload) const; - void send_sedp_heartbeats_to(const std::string &dest_address, uint16_t dest_port, - const GuidPrefix &dest_prefix, size_t writer_count, - size_t reader_count); - // Writer-side ACKNACK response: resend the requested sequence numbers to the requesting reader. - void retransmit_user_data(const GuidPrefix &reader_prefix, EntityId reader_id, EntityId writer_id, - const std::vector &requested_sequence_numbers); - void retransmit_sedp(const GuidPrefix &reader_prefix, EntityId reader_id, EntityId writer_id, - const std::vector &requested_sequence_numbers); - // Reader-side GAP handling: mark the irrelevant sequence numbers as skipped and advance. - // [gap_start, gap_list_base) is the contiguous irrelevant range; bitmap_irrelevant are individual - // irrelevant sequence numbers >= gap_list_base. - void handle_user_gap(const GuidPrefix &writer_prefix, EntityId writer_id, int64_t gap_start, - int64_t gap_list_base, const std::vector &bitmap_irrelevant); - void handle_builtin_gap(const GuidPrefix &writer_prefix, EntityId writer_id, int64_t gap_start, - int64_t gap_list_base, const std::vector &bitmap_irrelevant); - - /// @brief Per-writer reliable-QoS state: the history of recently-sent samples - /// (keyed by sequence number) plus heartbeat bookkeeping. Guarded by - /// reliable_mutex_. - struct WriterReliableState { - std::map> - history{}; ///< Cached CDR payloads keyed by sequence number. - int64_t last_sequence_number{0}; ///< Highest sequence number written so far (0 = none). - uint32_t heartbeat_count{0}; ///< Monotonic HEARTBEAT count for this writer. - }; - - /// @brief Per-(local reader, remote writer) reliable-QoS receive state used for - /// duplicate suppression, in-order delivery, and ACKNACK generation. - /// Guarded by reliable_mutex_. - struct ReaderReliableState { - int64_t highest_delivered{0}; ///< Highest sequence number delivered in order (0 = none). - std::map> - reorder{}; ///< Out-of-order samples awaiting their predecessors. - std::set irrelevant{}; ///< Out-of-order sequence numbers a GAP marked irrelevant - ///< (skipped, not delivered, when the frontier reaches them). - uint32_t last_heartbeat_count{0}; ///< Highest HEARTBEAT count seen (stale-heartbeat detection). - uint32_t acknack_count{0}; ///< Monotonic ACKNACK count emitted by this reader. - }; - - // Advance the reader's in-order frontier over any now-contiguous buffered samples (collected into - // `delivered`) and irrelevant (GAP-skipped) sequence numbers. Caller must hold reliable_mutex_. - static void drain_reader_frontier(ReaderReliableState &state, - std::vector> &delivered); - // Mark a GAP's irrelevant sequence numbers as skipped, then drain the frontier. Caller must hold - // reliable_mutex_. - static void apply_gap(ReaderReliableState &state, int64_t gap_start, int64_t gap_list_base, - const std::vector &bitmap_irrelevant, - std::vector> &delivered); - - /// @brief Hash functor for using a Guid as an unordered_map key. - struct GuidHash { - size_t operator()(const Guid &guid) const; - }; - - /// @brief Owns the discovered-participant and discovered-endpoint records and - /// the lock that guards them. - /// @details Identity is the GUID. Updates *merge* the incoming fields into the - /// existing record (the caller's apply mutator sets only the fields - /// actually present in the received message), so a later announcement - /// that omits a locator/QoS/name does not erase previously-learned - /// values. Centralizing storage + merge + locking here keeps the - /// participant's discovery bookkeeping in one place (and is the - /// natural home for future lease-based expiry). - class DiscoveryDb { - public: - /// @brief Result of an upsert: whether the record was newly created and a - /// snapshot of the merged record. - template struct UpsertResult { - bool is_new{false}; - Proxy value{}; - }; - - UpsertResult - upsert_participant(const Guid &participant_guid, - const std::function &apply); - UpsertResult upsert_endpoint(bool is_reader, const Guid &endpoint_guid, - const std::function &apply); - - std::optional find_participant_by_prefix(const GuidPrefix &prefix) const; - std::optional find_writer(const Guid &guid) const; - - std::vector participants() const; - std::vector writers() const; - std::vector readers() const; - - void clear(); - - private: - mutable std::mutex mutex_; - std::unordered_map participants_; - std::unordered_map writers_; - std::unordered_map readers_; - }; - - std::vector build_data_message_with_sequence_number(const WriterConfig &writer_config, - std::span cdr_payload, - int64_t sequence_number, - EntityId reader_id) const; - void store_reliable_sample(const WriterConfig &writer_config, int64_t sequence_number, - std::span cdr_payload); - bool send_heartbeat_for_writer(const WriterConfig &writer_config); - bool send_heartbeats_now(); - void deliver_reliable_sample(uint32_t reader_entity_index, const Guid &writer_guid, - int64_t sequence_number, std::span payload, - const std::function)> &on_sample); - void send_acknack_for_heartbeat(const GuidPrefix &writer_prefix, const EntityId &writer_id, - int64_t first_sn, int64_t last_sn, uint32_t heartbeat_count, - bool heartbeat_final); - // Builtin (SPDP/SEDP) reliability: track received discovery samples and ACKNACK the heartbeats a - // reliable peer (e.g. Fast DDS) sends for its builtin SEDP writers, so it (re)sends us the - // endpoint discovery data we need to match it. - void record_builtin_sample(const Guid &writer_guid, int64_t sequence_number); - void send_builtin_acknack(const GuidPrefix &writer_prefix, const EntityId &writer_id, - int64_t first_sn, int64_t last_sn, uint32_t heartbeat_count, - bool heartbeat_final); - - Config config_; - GuidPrefix guid_prefix_{}; - std::atomic_bool started_{false}; - - std::unique_ptr metatraffic_multicast_receiver_; - std::unique_ptr metatraffic_unicast_receiver_; - std::vector user_multicast_receivers_; - std::unique_ptr user_unicast_receiver_; - std::unique_ptr announce_task_; - std::unique_ptr heartbeat_task_; - - mutable std::mutex mutex_; - mutable std::mutex receivers_mutex_; ///< Guards user_multicast_receivers_ against concurrent - ///< add_reader()/stop() access. - mutable std::mutex sequence_mutex_; - mutable std::atomic spdp_sequence_number_{1}; - mutable std::atomic sedp_publications_sequence_number_{1}; - mutable std::atomic sedp_subscriptions_sequence_number_{1}; - mutable std::unordered_map user_data_sequence_numbers_; - mutable std::mutex reliable_mutex_; ///< Guards writer_reliable_states_ against concurrent - ///< publish()/heartbeat/receive access. - std::unordered_map - writer_reliable_states_; ///< Reliable-QoS state keyed by writer entity_index. - std::unordered_map - reader_reliable_states_; ///< Reliable-QoS receive state keyed by "#". - std::unordered_map - builtin_reader_states_; ///< Received-SEDP-sample state keyed by remote builtin writer GUID, - ///< used to ACKNACK a reliable peer's discovery heartbeats. - mutable std::atomic sedp_pub_heartbeat_count_{0}; ///< HEARTBEAT count for our builtin - ///< SEDP publications writer. - mutable std::atomic sedp_sub_heartbeat_count_{0}; ///< HEARTBEAT count for our builtin - ///< SEDP subscriptions writer. - std::vector writers_; - std::vector readers_; - DiscoveryDb discovery_; ///< Discovered participants + endpoints (owns its own lock). -}; -} // namespace espp diff --git a/components/rtps_embedded/include/rtps/common/Locator.hpp b/components/rtps/include/rtps/common/Locator.hpp similarity index 100% rename from components/rtps_embedded/include/rtps/common/Locator.hpp rename to components/rtps/include/rtps/common/Locator.hpp diff --git a/components/rtps_embedded/include/rtps/common/types.hpp b/components/rtps/include/rtps/common/types.hpp similarity index 100% rename from components/rtps_embedded/include/rtps/common/types.hpp rename to components/rtps/include/rtps/common/types.hpp diff --git a/components/rtps_embedded/include/rtps/communication/EsppTransport.hpp b/components/rtps/include/rtps/communication/EsppTransport.hpp similarity index 100% rename from components/rtps_embedded/include/rtps/communication/EsppTransport.hpp rename to components/rtps/include/rtps/communication/EsppTransport.hpp diff --git a/components/rtps_embedded/include/rtps/communication/PacketInfo.hpp b/components/rtps/include/rtps/communication/PacketInfo.hpp similarity index 100% rename from components/rtps_embedded/include/rtps/communication/PacketInfo.hpp rename to components/rtps/include/rtps/communication/PacketInfo.hpp diff --git a/components/rtps_embedded/include/rtps/config.hpp b/components/rtps/include/rtps/config.hpp similarity index 100% rename from components/rtps_embedded/include/rtps/config.hpp rename to components/rtps/include/rtps/config.hpp diff --git a/components/rtps_embedded/include/rtps/config_desktop.hpp b/components/rtps/include/rtps/config_desktop.hpp similarity index 100% rename from components/rtps_embedded/include/rtps/config_desktop.hpp rename to components/rtps/include/rtps/config_desktop.hpp diff --git a/components/rtps_embedded/include/rtps/config_esp32.hpp b/components/rtps/include/rtps/config_esp32.hpp similarity index 100% rename from components/rtps_embedded/include/rtps/config_esp32.hpp rename to components/rtps/include/rtps/config_esp32.hpp diff --git a/components/rtps_embedded/include/rtps/config_host_large.hpp b/components/rtps/include/rtps/config_host_large.hpp similarity index 100% rename from components/rtps_embedded/include/rtps/config_host_large.hpp rename to components/rtps/include/rtps/config_host_large.hpp diff --git a/components/rtps_embedded/include/rtps/discovery/BuiltInEndpoints.hpp b/components/rtps/include/rtps/discovery/BuiltInEndpoints.hpp similarity index 100% rename from components/rtps_embedded/include/rtps/discovery/BuiltInEndpoints.hpp rename to components/rtps/include/rtps/discovery/BuiltInEndpoints.hpp diff --git a/components/rtps_embedded/include/rtps/discovery/ParticipantProxyData.hpp b/components/rtps/include/rtps/discovery/ParticipantProxyData.hpp similarity index 100% rename from components/rtps_embedded/include/rtps/discovery/ParticipantProxyData.hpp rename to components/rtps/include/rtps/discovery/ParticipantProxyData.hpp diff --git a/components/rtps_embedded/include/rtps/discovery/SEDPAgent.hpp b/components/rtps/include/rtps/discovery/SEDPAgent.hpp similarity index 100% rename from components/rtps_embedded/include/rtps/discovery/SEDPAgent.hpp rename to components/rtps/include/rtps/discovery/SEDPAgent.hpp diff --git a/components/rtps_embedded/include/rtps/discovery/SPDPAgent.hpp b/components/rtps/include/rtps/discovery/SPDPAgent.hpp similarity index 100% rename from components/rtps_embedded/include/rtps/discovery/SPDPAgent.hpp rename to components/rtps/include/rtps/discovery/SPDPAgent.hpp diff --git a/components/rtps_embedded/include/rtps/discovery/TopicData.hpp b/components/rtps/include/rtps/discovery/TopicData.hpp similarity index 100% rename from components/rtps_embedded/include/rtps/discovery/TopicData.hpp rename to components/rtps/include/rtps/discovery/TopicData.hpp diff --git a/components/rtps_embedded/include/rtps/entities/Domain.hpp b/components/rtps/include/rtps/entities/Domain.hpp similarity index 100% rename from components/rtps_embedded/include/rtps/entities/Domain.hpp rename to components/rtps/include/rtps/entities/Domain.hpp diff --git a/components/rtps_embedded/include/rtps/entities/Participant.hpp b/components/rtps/include/rtps/entities/Participant.hpp similarity index 100% rename from components/rtps_embedded/include/rtps/entities/Participant.hpp rename to components/rtps/include/rtps/entities/Participant.hpp diff --git a/components/rtps_embedded/include/rtps/entities/Reader.hpp b/components/rtps/include/rtps/entities/Reader.hpp similarity index 100% rename from components/rtps_embedded/include/rtps/entities/Reader.hpp rename to components/rtps/include/rtps/entities/Reader.hpp diff --git a/components/rtps_embedded/include/rtps/entities/ReaderProxy.hpp b/components/rtps/include/rtps/entities/ReaderProxy.hpp similarity index 100% rename from components/rtps_embedded/include/rtps/entities/ReaderProxy.hpp rename to components/rtps/include/rtps/entities/ReaderProxy.hpp diff --git a/components/rtps_embedded/include/rtps/entities/StatefulReader.hpp b/components/rtps/include/rtps/entities/StatefulReader.hpp similarity index 100% rename from components/rtps_embedded/include/rtps/entities/StatefulReader.hpp rename to components/rtps/include/rtps/entities/StatefulReader.hpp diff --git a/components/rtps_embedded/include/rtps/entities/StatefulWriter.hpp b/components/rtps/include/rtps/entities/StatefulWriter.hpp similarity index 100% rename from components/rtps_embedded/include/rtps/entities/StatefulWriter.hpp rename to components/rtps/include/rtps/entities/StatefulWriter.hpp diff --git a/components/rtps_embedded/include/rtps/entities/StatelessReader.hpp b/components/rtps/include/rtps/entities/StatelessReader.hpp similarity index 100% rename from components/rtps_embedded/include/rtps/entities/StatelessReader.hpp rename to components/rtps/include/rtps/entities/StatelessReader.hpp diff --git a/components/rtps_embedded/include/rtps/entities/StatelessWriter.hpp b/components/rtps/include/rtps/entities/StatelessWriter.hpp similarity index 100% rename from components/rtps_embedded/include/rtps/entities/StatelessWriter.hpp rename to components/rtps/include/rtps/entities/StatelessWriter.hpp diff --git a/components/rtps_embedded/include/rtps/entities/Writer.hpp b/components/rtps/include/rtps/entities/Writer.hpp similarity index 100% rename from components/rtps_embedded/include/rtps/entities/Writer.hpp rename to components/rtps/include/rtps/entities/Writer.hpp diff --git a/components/rtps_embedded/include/rtps/entities/WriterProxy.hpp b/components/rtps/include/rtps/entities/WriterProxy.hpp similarity index 100% rename from components/rtps_embedded/include/rtps/entities/WriterProxy.hpp rename to components/rtps/include/rtps/entities/WriterProxy.hpp diff --git a/components/rtps_embedded/include/rtps/messages/MessageFactory.hpp b/components/rtps/include/rtps/messages/MessageFactory.hpp similarity index 100% rename from components/rtps_embedded/include/rtps/messages/MessageFactory.hpp rename to components/rtps/include/rtps/messages/MessageFactory.hpp diff --git a/components/rtps_embedded/include/rtps/messages/MessageReceiver.hpp b/components/rtps/include/rtps/messages/MessageReceiver.hpp similarity index 100% rename from components/rtps_embedded/include/rtps/messages/MessageReceiver.hpp rename to components/rtps/include/rtps/messages/MessageReceiver.hpp diff --git a/components/rtps_embedded/include/rtps/messages/MessageTypes.hpp b/components/rtps/include/rtps/messages/MessageTypes.hpp similarity index 100% rename from components/rtps_embedded/include/rtps/messages/MessageTypes.hpp rename to components/rtps/include/rtps/messages/MessageTypes.hpp diff --git a/components/rtps_embedded/include/rtps/rpc/action_naming.hpp b/components/rtps/include/rtps/rpc/action_naming.hpp similarity index 100% rename from components/rtps_embedded/include/rtps/rpc/action_naming.hpp rename to components/rtps/include/rtps/rpc/action_naming.hpp diff --git a/components/rtps_embedded/include/rtps/rpc/action_types.hpp b/components/rtps/include/rtps/rpc/action_types.hpp similarity index 100% rename from components/rtps_embedded/include/rtps/rpc/action_types.hpp rename to components/rtps/include/rtps/rpc/action_types.hpp diff --git a/components/rtps_embedded/include/rtps/rpc/native_protocol.hpp b/components/rtps/include/rtps/rpc/native_protocol.hpp similarity index 100% rename from components/rtps_embedded/include/rtps/rpc/native_protocol.hpp rename to components/rtps/include/rtps/rpc/native_protocol.hpp diff --git a/components/rtps_embedded/include/rtps/rpc/sample_identity.hpp b/components/rtps/include/rtps/rpc/sample_identity.hpp similarity index 100% rename from components/rtps_embedded/include/rtps/rpc/sample_identity.hpp rename to components/rtps/include/rtps/rpc/sample_identity.hpp diff --git a/components/rtps_embedded/include/rtps/rpc/service_naming.hpp b/components/rtps/include/rtps/rpc/service_naming.hpp similarity index 100% rename from components/rtps_embedded/include/rtps/rpc/service_naming.hpp rename to components/rtps/include/rtps/rpc/service_naming.hpp diff --git a/components/rtps_embedded/include/rtps/rtps.hpp b/components/rtps/include/rtps/rtps.hpp similarity index 100% rename from components/rtps_embedded/include/rtps/rtps.hpp rename to components/rtps/include/rtps/rtps.hpp diff --git a/components/rtps_embedded/include/rtps/storages/CacheChange.hpp b/components/rtps/include/rtps/storages/CacheChange.hpp similarity index 100% rename from components/rtps_embedded/include/rtps/storages/CacheChange.hpp rename to components/rtps/include/rtps/storages/CacheChange.hpp diff --git a/components/rtps_embedded/include/rtps/storages/HistoryCacheWithDeletion.hpp b/components/rtps/include/rtps/storages/HistoryCacheWithDeletion.hpp similarity index 100% rename from components/rtps_embedded/include/rtps/storages/HistoryCacheWithDeletion.hpp rename to components/rtps/include/rtps/storages/HistoryCacheWithDeletion.hpp diff --git a/components/rtps_embedded/include/rtps/storages/MemoryPool.hpp b/components/rtps/include/rtps/storages/MemoryPool.hpp similarity index 100% rename from components/rtps_embedded/include/rtps/storages/MemoryPool.hpp rename to components/rtps/include/rtps/storages/MemoryPool.hpp diff --git a/components/rtps_embedded/include/rtps/storages/PayloadBuffer.hpp b/components/rtps/include/rtps/storages/PayloadBuffer.hpp similarity index 100% rename from components/rtps_embedded/include/rtps/storages/PayloadBuffer.hpp rename to components/rtps/include/rtps/storages/PayloadBuffer.hpp diff --git a/components/rtps_embedded/include/rtps/storages/SimpleHistoryCache.hpp b/components/rtps/include/rtps/storages/SimpleHistoryCache.hpp similarity index 100% rename from components/rtps_embedded/include/rtps/storages/SimpleHistoryCache.hpp rename to components/rtps/include/rtps/storages/SimpleHistoryCache.hpp diff --git a/components/rtps_embedded/include/rtps/storages/StorageArray.hpp b/components/rtps/include/rtps/storages/StorageArray.hpp similarity index 100% rename from components/rtps_embedded/include/rtps/storages/StorageArray.hpp rename to components/rtps/include/rtps/storages/StorageArray.hpp diff --git a/components/rtps_embedded/include/rtps/storages/ThreadSafeCircularBuffer.hpp b/components/rtps/include/rtps/storages/ThreadSafeCircularBuffer.hpp similarity index 100% rename from components/rtps_embedded/include/rtps/storages/ThreadSafeCircularBuffer.hpp rename to components/rtps/include/rtps/storages/ThreadSafeCircularBuffer.hpp diff --git a/components/rtps_embedded/include/rtps/storages/ThreadSafeCircularBuffer.tpp b/components/rtps/include/rtps/storages/ThreadSafeCircularBuffer.tpp similarity index 100% rename from components/rtps_embedded/include/rtps/storages/ThreadSafeCircularBuffer.tpp rename to components/rtps/include/rtps/storages/ThreadSafeCircularBuffer.tpp diff --git a/components/rtps_embedded/include/rtps/utils/CdrBuffer.hpp b/components/rtps/include/rtps/utils/CdrBuffer.hpp similarity index 98% rename from components/rtps_embedded/include/rtps/utils/CdrBuffer.hpp rename to components/rtps/include/rtps/utils/CdrBuffer.hpp index 741be9a35d..234d54c873 100644 --- a/components/rtps_embedded/include/rtps/utils/CdrBuffer.hpp +++ b/components/rtps/include/rtps/utils/CdrBuffer.hpp @@ -30,7 +30,7 @@ This file is part of embeddedRTPS. // - raw byte arrays written/read without alignment, // - fixed caller-owned buffers (span_sink), no heap. // Used for the SPDP/SEDP PL_CDR discovery parameter lists; the byte-identity -// of the output is frozen by pc/tests/rtps_embedded_golden.cpp. +// of the output is frozen by pc/tests/rtps_golden.cpp. #include #include diff --git a/components/rtps_embedded/include/rtps/utils/Diagnostics.hpp b/components/rtps/include/rtps/utils/Diagnostics.hpp similarity index 100% rename from components/rtps_embedded/include/rtps/utils/Diagnostics.hpp rename to components/rtps/include/rtps/utils/Diagnostics.hpp diff --git a/components/rtps_embedded/include/rtps/utils/Log.hpp b/components/rtps/include/rtps/utils/Log.hpp similarity index 100% rename from components/rtps_embedded/include/rtps/utils/Log.hpp rename to components/rtps/include/rtps/utils/Log.hpp diff --git a/components/rtps_embedded/include/rtps/utils/constants.hpp b/components/rtps/include/rtps/utils/constants.hpp similarity index 100% rename from components/rtps_embedded/include/rtps/utils/constants.hpp rename to components/rtps/include/rtps/utils/constants.hpp diff --git a/components/rtps_embedded/include/rtps/utils/hash.hpp b/components/rtps/include/rtps/utils/hash.hpp similarity index 100% rename from components/rtps_embedded/include/rtps/utils/hash.hpp rename to components/rtps/include/rtps/utils/hash.hpp diff --git a/components/rtps_embedded/include/rtps/utils/printutils.hpp b/components/rtps/include/rtps/utils/printutils.hpp similarity index 100% rename from components/rtps_embedded/include/rtps/utils/printutils.hpp rename to components/rtps/include/rtps/utils/printutils.hpp diff --git a/components/rtps_embedded/include/rtps/utils/sysFunctions.hpp b/components/rtps/include/rtps/utils/sysFunctions.hpp similarity index 100% rename from components/rtps_embedded/include/rtps/utils/sysFunctions.hpp rename to components/rtps/include/rtps/utils/sysFunctions.hpp diff --git a/components/rtps_embedded/include/rtps/utils/udpUtils.hpp b/components/rtps/include/rtps/utils/udpUtils.hpp similarity index 100% rename from components/rtps_embedded/include/rtps/utils/udpUtils.hpp rename to components/rtps/include/rtps/utils/udpUtils.hpp diff --git a/components/rtps_embedded/include/rtps_action.hpp b/components/rtps/include/rtps_action.hpp similarity index 100% rename from components/rtps_embedded/include/rtps_action.hpp rename to components/rtps/include/rtps_action.hpp diff --git a/components/rtps_embedded/include/rtps_message.hpp b/components/rtps/include/rtps_message.hpp similarity index 100% rename from components/rtps_embedded/include/rtps_message.hpp rename to components/rtps/include/rtps_message.hpp diff --git a/components/rtps_embedded/include/rtps_participant.hpp b/components/rtps/include/rtps_participant.hpp similarity index 99% rename from components/rtps_embedded/include/rtps_participant.hpp rename to components/rtps/include/rtps_participant.hpp index 20aadeaf76..ae7dac7f8e 100644 --- a/components/rtps_embedded/include/rtps_participant.hpp +++ b/components/rtps/include/rtps_participant.hpp @@ -18,7 +18,7 @@ #include "base_component.hpp" // Forward declarations of the embeddedRTPS engine types (see -// components/rtps_embedded/include/rtps/). The engine headers are only needed +// components/rtps/include/rtps/). The engine headers are only needed // by the implementation; users of this facade never touch them directly. namespace rtps { class Domain; @@ -41,7 +41,7 @@ namespace espp { /// @brief RTPS/DDS participant for pub/sub interop with FastDDS and ROS 2. /// /// An espp-idiomatic facade over the embeddedRTPS engine (the FastDDS/ROS 2 -/// interop-proven RTPS implementation vendored in components/rtps_embedded). +/// interop-proven RTPS implementation vendored in components/rtps). /// One RtpsParticipant owns one RTPS domain participant: create it with a /// Config, start() it, then add writers/readers and publish CDR-encapsulated /// samples. Samples arriving on readers are delivered via the on_sample @@ -53,7 +53,7 @@ namespace espp { /// "std_msgs::msg::dds_::String_" matches a ROS 2 std_msgs/String subscriber /// on /chatter). /// -/// Phase 1 facade (see components/rtps_embedded/REFACTOR_PLAN.md): the engine +/// 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 diff --git a/components/rtps_embedded/include/rtps_pubsub.hpp b/components/rtps/include/rtps_pubsub.hpp similarity index 100% rename from components/rtps_embedded/include/rtps_pubsub.hpp rename to components/rtps/include/rtps_pubsub.hpp diff --git a/components/rtps_embedded/include/rtps_service.hpp b/components/rtps/include/rtps_service.hpp similarity index 100% rename from components/rtps_embedded/include/rtps_service.hpp rename to components/rtps/include/rtps_service.hpp diff --git a/components/rtps_embedded/interop/Dockerfile b/components/rtps/interop/Dockerfile similarity index 100% rename from components/rtps_embedded/interop/Dockerfile rename to components/rtps/interop/Dockerfile diff --git a/components/rtps_embedded/interop/README.md b/components/rtps/interop/README.md similarity index 91% rename from components/rtps_embedded/interop/README.md rename to components/rtps/interop/README.md index 55eefce560..e6d08b36ed 100644 --- a/components/rtps_embedded/interop/README.md +++ b/components/rtps/interop/README.md @@ -6,7 +6,7 @@ this matrix green. ## Run ```bash -cd components/rtps_embedded/interop +cd components/rtps/interop ./run.sh ``` @@ -20,7 +20,7 @@ before building, so your host `lib/pc` artifacts are never touched. | test | what it proves | |---|---| | build | the engine + espp lib build on linux | -| golden | wire-format bytes unchanged (see `pc/tests/rtps_embedded_golden.cpp`) | +| golden | wire-format bytes unchanged (see `pc/tests/rtps_golden.cpp`) | | loopback | espp<->espp discovery + delivery in one process | | espp_pub->ros2_echo | espp reliable writer -> ROS 2 subscriber (`rt/chatter`, `std_msgs::msg::dds_::String_`) | | ros2_pub->espp_sub | ROS 2 publisher -> espp reliable reader | diff --git a/components/rtps_embedded/interop/ros2_add_two_ints_server.py b/components/rtps/interop/ros2_add_two_ints_server.py similarity index 100% rename from components/rtps_embedded/interop/ros2_add_two_ints_server.py rename to components/rtps/interop/ros2_add_two_ints_server.py diff --git a/components/rtps_embedded/interop/ros2_big_publisher.py b/components/rtps/interop/ros2_big_publisher.py similarity index 100% rename from components/rtps_embedded/interop/ros2_big_publisher.py rename to components/rtps/interop/ros2_big_publisher.py diff --git a/components/rtps_embedded/interop/ros2_fibonacci_server.py b/components/rtps/interop/ros2_fibonacci_server.py similarity index 100% rename from components/rtps_embedded/interop/ros2_fibonacci_server.py rename to components/rtps/interop/ros2_fibonacci_server.py diff --git a/components/rtps_embedded/interop/run.sh b/components/rtps/interop/run.sh similarity index 68% rename from components/rtps_embedded/interop/run.sh rename to components/rtps/interop/run.sh index ddb64667a4..98cfb780bc 100755 --- a/components/rtps_embedded/interop/run.sh +++ b/components/rtps/interop/run.sh @@ -1,9 +1,9 @@ #!/usr/bin/env bash # Host-side entry point: build the harness image and run the interop matrix. -# Usage: ./run.sh (from components/rtps_embedded/interop) +# Usage: ./run.sh (from components/rtps/interop) set -euo pipefail cd "$(dirname "$0")" REPO_ROOT="$(cd ../../.. && pwd)" docker build -t espp-rtps-interop . exec docker run --rm -v "$REPO_ROOT":/work espp-rtps-interop \ - bash /work/components/rtps_embedded/interop/run_interop.sh + bash /work/components/rtps/interop/run_interop.sh diff --git a/components/rtps_embedded/interop/run_interop.sh b/components/rtps/interop/run_interop.sh similarity index 88% rename from components/rtps_embedded/interop/run_interop.sh rename to components/rtps/interop/run_interop.sh index 8e333619a2..77a4d5ca1f 100755 --- a/components/rtps_embedded/interop/run_interop.sh +++ b/components/rtps/interop/run_interop.sh @@ -27,14 +27,14 @@ cmake -S lib -B lib/build -DCMAKE_BUILD_TYPE=Release > /tmp/cmake_lib.log 2>&1 \ && cmake --build lib/build -j"$(nproc)" --target install > /tmp/build_lib.log 2>&1 \ && cmake -S pc -B pc/build -DCMAKE_BUILD_TYPE=Release > /tmp/cmake.log 2>&1 \ && cmake --build pc/build -j"$(nproc)" --target \ - rtps_embedded_pubsub rtps_embedded_golden rtps_facade_pubsub rtps_typed_pubsub \ + rtps_pubsub rtps_golden rtps_facade_pubsub rtps_typed_pubsub \ rtps_facade_frag rtps_facade_backlog rtps_facade_frag_sizes rtps_service_loopback \ rtps_service_naming rtps_action_naming rtps_action_types rtps_native_protocol \ rtps_action_loopback \ 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_embedded_interop_pub rtps_embedded_interop_sub > /tmp/build.log 2>&1 + rtps_interop_pub rtps_interop_sub > /tmp/build.log 2>&1 build_rc=$? result "build" $build_rc if [ $build_rc -ne 0 ]; then tail -30 /tmp/cmake_lib.log /tmp/build_lib.log /tmp/build.log; exit 1; fi @@ -45,7 +45,7 @@ export ROS_DOMAIN_ID=0 # matches the engine's Config::DOMAIN_ID export RMW_IMPLEMENTATION=rmw_fastrtps_cpp note "Golden wire-format tests" -"$BIN"/rtps_embedded_golden; result "golden" $? +"$BIN"/rtps_golden; result "golden" $? note "ROS 2 service + action name/type mangling (host)" "$BIN"/rtps_service_naming; result "service_naming" $? @@ -58,7 +58,7 @@ note "native (espp<->espp) protocol codec: header + action framing (host)" "$BIN"/rtps_native_protocol; result "native_protocol" $? note "espp <-> espp in-process loopback" -"$BIN"/rtps_embedded_pubsub; result "loopback" $? +"$BIN"/rtps_pubsub; result "loopback" $? note "facade <-> facade in-process (two participants, port probing)" "$BIN"/rtps_facade_pubsub; result "facade_loopback" $? @@ -108,10 +108,10 @@ note "espp <-> espp cross-process 200 KB DATA_FRAG (reliable, large 60000 frag s # default large-fragment style; reliable QoS recovers any dropped fragment by # whole-sample retransmit. Publish slowly so each SN completes (single reassembly # slot) before the next. -"$BIN"/rtps_embedded_interop_sub xprocfrag std_msgs::msg::dds_::String_ 1 1 40 "" 200000 > /tmp/xsubfrag.log 2>&1 & +"$BIN"/rtps_interop_sub xprocfrag std_msgs::msg::dds_::String_ 1 1 40 "" 200000 > /tmp/xsubfrag.log 2>&1 & XSUBF=$! sleep 2 -"$BIN"/rtps_embedded_interop_pub xprocfrag std_msgs::msg::dds_::String_ 1 12 3000 "" 200000 60000 > /tmp/xpubfrag.log 2>&1 & +"$BIN"/rtps_interop_pub xprocfrag std_msgs::msg::dds_::String_ 1 12 3000 "" 200000 60000 > /tmp/xpubfrag.log 2>&1 & XPUBF=$! wait $XSUBF xsubf_rc=$? @@ -120,10 +120,10 @@ tail -2 /tmp/xsubfrag.log result "cross_process_frag_200k" $xsubf_rc note "espp <-> espp cross-process on one host (port probing)" -"$BIN"/rtps_embedded_interop_sub xproc std_msgs::msg::dds_::String_ 0 3 20 > /tmp/xsub.log 2>&1 & +"$BIN"/rtps_interop_sub xproc std_msgs::msg::dds_::String_ 0 3 20 > /tmp/xsub.log 2>&1 & XSUB=$! sleep 2 -"$BIN"/rtps_embedded_interop_pub xproc std_msgs::msg::dds_::String_ 0 30 200 > /tmp/xpub.log 2>&1 & +"$BIN"/rtps_interop_pub xproc std_msgs::msg::dds_::String_ 0 30 200 > /tmp/xpub.log 2>&1 & XPUB=$! wait $XSUB xsub_rc=$? @@ -132,7 +132,7 @@ tail -2 /tmp/xsub.log result "cross_process" $xsub_rc note "espp publisher -> ROS 2 subscriber (reliable, rt/chatter)" -"$BIN"/rtps_embedded_interop_pub rt/chatter std_msgs::msg::dds_::String_ 1 60 200 > /tmp/pub1.log 2>&1 & +"$BIN"/rtps_interop_pub rt/chatter std_msgs::msg::dds_::String_ 1 60 200 > /tmp/pub1.log 2>&1 & ESPP_PID=$! sleep 3 timeout 30 ros2 topic echo --once /chatter std_msgs/msg/String > /tmp/echo.log 2>&1 @@ -142,7 +142,7 @@ cat /tmp/echo.log result "espp_pub->ros2_echo" $echo_rc note "ROS 2 publisher -> espp subscriber (reliable, rt/chatter)" -"$BIN"/rtps_embedded_interop_sub rt/chatter std_msgs::msg::dds_::String_ 1 3 30 > /tmp/sub1.log 2>&1 & +"$BIN"/rtps_interop_sub rt/chatter std_msgs::msg::dds_::String_ 1 3 30 > /tmp/sub1.log 2>&1 & SUB_PID=$! sleep 3 timeout 35 ros2 topic pub -r 5 /chatter std_msgs/msg/String "data: 'ros2 to espp'" > /tmp/rospub.log 2>&1 & @@ -154,7 +154,7 @@ cat /tmp/sub1.log result "ros2_pub->espp_sub" $sub_rc note "espp best-effort publisher -> ROS 2 best-effort subscriber" -"$BIN"/rtps_embedded_interop_pub rt/chatter std_msgs::msg::dds_::String_ 0 60 200 > /tmp/pub2.log 2>&1 & +"$BIN"/rtps_interop_pub rt/chatter std_msgs::msg::dds_::String_ 0 60 200 > /tmp/pub2.log 2>&1 & ESPP_PID=$! sleep 3 timeout 30 ros2 topic echo --once --qos-reliability best_effort /chatter std_msgs/msg/String > /tmp/echo2.log 2>&1 @@ -166,7 +166,7 @@ note "espp best-effort publisher -> ROS 2 (200 KB String, DATA_FRAG)" # espp fragments the 200 KB sample into DATA_FRAG submessages; rmw_fastrtps # reassembles it. Best-effort (v1 scope): in the shared-netns container there is # ~no loss, so this proves the espp->ROS 2 DATA_FRAG WIRE encoding interops. -"$BIN"/rtps_embedded_interop_pub rt/bignum std_msgs::msg::dds_::String_ 0 20 2000 "" 200000 60000 > /tmp/pubbig.log 2>&1 & +"$BIN"/rtps_interop_pub rt/bignum std_msgs::msg::dds_::String_ 0 20 2000 "" 200000 60000 > /tmp/pubbig.log 2>&1 & ESPP_PID=$! sleep 3 timeout 45 ros2 topic echo --once --full-length --qos-reliability best_effort /bignum std_msgs/msg/String > /tmp/echobig.log 2>&1 @@ -185,12 +185,12 @@ note "ROS 2 publisher -> espp subscriber (200 KB String, DATA_FRAG both vendors) # ros2/FastDDS fragments the 200 KB String (its own fragment size); espp must # reassemble it byte-exact against the shared 'A'+(i%26) pattern. Best-effort # (v1): proves espp decodes FastDDS DATA_FRAG. Reliable frag recovery is v2. -"$BIN"/rtps_embedded_interop_sub rt/bignum2 std_msgs::msg::dds_::String_ 0 1 45 "" 200000 > /tmp/subbig.log 2>&1 & +"$BIN"/rtps_interop_sub rt/bignum2 std_msgs::msg::dds_::String_ 0 1 45 "" 200000 > /tmp/subbig.log 2>&1 & SUBBIG_PID=$! sleep 3 # 200 KB cannot be passed as a `ros2 topic pub` CLI arg (ARG_MAX); publish it # from a small rclpy node that generates the pattern in-process (best-effort). -timeout 45 python3 /work/components/rtps_embedded/interop/ros2_big_publisher.py /bignum2 200000 40 > /tmp/rospubbig.log 2>&1 & +timeout 45 python3 /work/components/rtps/interop/ros2_big_publisher.py /bignum2 200000 40 > /tmp/rospubbig.log 2>&1 & ROSBIG_PID=$! wait $SUBBIG_PID big2_rc=$? @@ -221,7 +221,7 @@ echo "--- espp server ---"; grep "server:" /tmp/svcsrv.log | tail -3 result "espp_service_server<-ros2_client" $svccall_rc note "espp service client -> ROS 2 server (rclpy add_two_ints)" -python3 /work/components/rtps_embedded/interop/ros2_add_two_ints_server.py > /tmp/rossvcsrv.log 2>&1 & +python3 /work/components/rtps/interop/ros2_add_two_ints_server.py > /tmp/rossvcsrv.log 2>&1 & ROSSVC=$! sleep 4 timeout 35 "$BIN"/rtps_service_interop_client /add_two_ints example_interfaces::srv::dds_::AddTwoInts \ @@ -253,7 +253,7 @@ echo "--- espp action server ---"; grep "server:" /tmp/actsrv.log | tail -3 result "espp_action_server<-ros2_client" $actsend_rc note "espp action client -> ROS 2 server (rclpy Fibonacci)" -python3 /work/components/rtps_embedded/interop/ros2_fibonacci_server.py > /tmp/rosactsrv.log 2>&1 & +python3 /work/components/rtps/interop/ros2_fibonacci_server.py > /tmp/rosactsrv.log 2>&1 & ROSACT=$! sleep 5 timeout 40 "$BIN"/rtps_action_interop_client /fibonacci example_interfaces::action::dds_::Fibonacci \ diff --git a/components/rtps_embedded/src/communication/EsppTransport.cpp b/components/rtps/src/communication/EsppTransport.cpp similarity index 100% rename from components/rtps_embedded/src/communication/EsppTransport.cpp rename to components/rtps/src/communication/EsppTransport.cpp diff --git a/components/rtps_embedded/src/discovery/ParticipantProxyData.cpp b/components/rtps/src/discovery/ParticipantProxyData.cpp similarity index 100% rename from components/rtps_embedded/src/discovery/ParticipantProxyData.cpp rename to components/rtps/src/discovery/ParticipantProxyData.cpp diff --git a/components/rtps_embedded/src/discovery/SEDPAgent.cpp b/components/rtps/src/discovery/SEDPAgent.cpp similarity index 100% rename from components/rtps_embedded/src/discovery/SEDPAgent.cpp rename to components/rtps/src/discovery/SEDPAgent.cpp diff --git a/components/rtps_embedded/src/discovery/SPDPAgent.cpp b/components/rtps/src/discovery/SPDPAgent.cpp similarity index 100% rename from components/rtps_embedded/src/discovery/SPDPAgent.cpp rename to components/rtps/src/discovery/SPDPAgent.cpp diff --git a/components/rtps_embedded/src/discovery/TopicData.cpp b/components/rtps/src/discovery/TopicData.cpp similarity index 100% rename from components/rtps_embedded/src/discovery/TopicData.cpp rename to components/rtps/src/discovery/TopicData.cpp diff --git a/components/rtps_embedded/src/entities/Domain.cpp b/components/rtps/src/entities/Domain.cpp similarity index 100% rename from components/rtps_embedded/src/entities/Domain.cpp rename to components/rtps/src/entities/Domain.cpp diff --git a/components/rtps_embedded/src/entities/Participant.cpp b/components/rtps/src/entities/Participant.cpp similarity index 100% rename from components/rtps_embedded/src/entities/Participant.cpp rename to components/rtps/src/entities/Participant.cpp diff --git a/components/rtps_embedded/src/entities/Reader.cpp b/components/rtps/src/entities/Reader.cpp similarity index 100% rename from components/rtps_embedded/src/entities/Reader.cpp rename to components/rtps/src/entities/Reader.cpp diff --git a/components/rtps_embedded/src/entities/StatefulReader.cpp b/components/rtps/src/entities/StatefulReader.cpp similarity index 100% rename from components/rtps_embedded/src/entities/StatefulReader.cpp rename to components/rtps/src/entities/StatefulReader.cpp diff --git a/components/rtps_embedded/src/entities/StatefulWriter.cpp b/components/rtps/src/entities/StatefulWriter.cpp similarity index 100% rename from components/rtps_embedded/src/entities/StatefulWriter.cpp rename to components/rtps/src/entities/StatefulWriter.cpp diff --git a/components/rtps_embedded/src/entities/StatelessReader.cpp b/components/rtps/src/entities/StatelessReader.cpp similarity index 100% rename from components/rtps_embedded/src/entities/StatelessReader.cpp rename to components/rtps/src/entities/StatelessReader.cpp diff --git a/components/rtps_embedded/src/entities/StatelessWriter.cpp b/components/rtps/src/entities/StatelessWriter.cpp similarity index 100% rename from components/rtps_embedded/src/entities/StatelessWriter.cpp rename to components/rtps/src/entities/StatelessWriter.cpp diff --git a/components/rtps_embedded/src/entities/Writer.cpp b/components/rtps/src/entities/Writer.cpp similarity index 100% rename from components/rtps_embedded/src/entities/Writer.cpp rename to components/rtps/src/entities/Writer.cpp diff --git a/components/rtps_embedded/src/messages/MessageReceiver.cpp b/components/rtps/src/messages/MessageReceiver.cpp similarity index 100% rename from components/rtps_embedded/src/messages/MessageReceiver.cpp rename to components/rtps/src/messages/MessageReceiver.cpp diff --git a/components/rtps_embedded/src/messages/MessageTypes.cpp b/components/rtps/src/messages/MessageTypes.cpp similarity index 100% rename from components/rtps_embedded/src/messages/MessageTypes.cpp rename to components/rtps/src/messages/MessageTypes.cpp diff --git a/components/rtps/src/rtps.cpp b/components/rtps/src/rtps.cpp deleted file mode 100644 index 8891aa62db..0000000000 --- a/components/rtps/src/rtps.cpp +++ /dev/null @@ -1,2926 +0,0 @@ -#include "rtps.hpp" - -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#if defined(ESP_PLATFORM) -#include "esp_random.h" -#else -#include -#endif - -#include "cdr.hpp" - -namespace { -constexpr std::array kRtpsMagic{'R', 'T', 'P', 'S'}; - -// Per-instance entropy mixed into the participant GUID so a restarted participant presents a new -// GUID. Without this a restart reuses the same writer GUID and republishes sequence numbers from 1, -// which reliable DDS/ROS 2 peers treat as already-seen duplicates and drop. -uint64_t random_guid_entropy() { -#if defined(ESP_PLATFORM) - return (static_cast(esp_random()) << 32) ^ esp_random(); -#else - std::random_device device; - uint64_t value = (static_cast(device()) << 32) ^ device(); - // Mix in a high-resolution clock sample so rapid restarts diverge even if random_device repeats. - value ^= - static_cast(std::chrono::high_resolution_clock::now().time_since_epoch().count()); - return value; -#endif -} - -constexpr uint16_t kPortBase = 7400; -constexpr uint16_t kDomainGain = 250; -constexpr uint16_t kParticipantGain = 2; -constexpr uint16_t kMetatrafficMulticastOffset = 0; -constexpr uint16_t kMetatrafficUnicastOffset = 10; -constexpr uint16_t kUserMulticastOffset = 1; -constexpr uint16_t kUserUnicastOffset = 11; - -constexpr uint8_t kSubmessageFlagLittleEndian = 0x01; -constexpr uint8_t kSubmessageFlagInlineQos = 0x02; -constexpr uint8_t kSubmessageFlagData = 0x04; -// For HEARTBEAT and ACKNACK submessages, bit 1 is the Final flag and bit 2 is -// the Liveliness flag (the InlineQos/Data bits above are DATA-specific). -constexpr uint8_t kSubmessageFlagFinal = 0x02; -constexpr uint8_t kSubmessageFlagLiveliness = 0x04; -constexpr uint16_t kDataSubmessageOctetsToInlineQos = 16; - -constexpr uint32_t kBuiltinEndpointParticipantAnnouncer = 1u << 0; -constexpr uint32_t kBuiltinEndpointParticipantDetector = 1u << 1; -constexpr uint32_t kBuiltinEndpointPublicationAnnouncer = 1u << 2; -constexpr uint32_t kBuiltinEndpointPublicationDetector = 1u << 3; -constexpr uint32_t kBuiltinEndpointSubscriptionAnnouncer = 1u << 4; -constexpr uint32_t kBuiltinEndpointSubscriptionDetector = 1u << 5; -constexpr uint32_t kBuiltinEndpointParticipantMessageWriter = 1u << 10; -constexpr uint32_t kBuiltinEndpointParticipantMessageReader = 1u << 11; -constexpr uint32_t kBuiltinEndpointSet = - kBuiltinEndpointParticipantAnnouncer | kBuiltinEndpointParticipantDetector | - kBuiltinEndpointPublicationAnnouncer | kBuiltinEndpointPublicationDetector | - kBuiltinEndpointSubscriptionAnnouncer | kBuiltinEndpointSubscriptionDetector; - -constexpr std::array kEntityIdUnknown{{0x00, 0x00, 0x00, 0x00}}; -constexpr std::array kParticipantEntityId{{0x00, 0x00, 0x01, 0xc1}}; -constexpr std::array kSpdpWriterEntityId{{0x00, 0x01, 0x00, 0xc2}}; -constexpr std::array kSpdpReaderEntityId{{0x00, 0x01, 0x00, 0xc7}}; -constexpr std::array kSedpPublicationsWriterEntityId{{0x00, 0x00, 0x03, 0xc2}}; -constexpr std::array kSedpPublicationsReaderEntityId{{0x00, 0x00, 0x03, 0xc7}}; -constexpr std::array kSedpSubscriptionsWriterEntityId{{0x00, 0x00, 0x04, 0xc2}}; -constexpr std::array kSedpSubscriptionsReaderEntityId{{0x00, 0x00, 0x04, 0xc7}}; -constexpr uint8_t kUserWriterNoKeyKind = 0x03; -constexpr uint8_t kUserReaderNoKeyKind = 0x04; - -constexpr uint32_t kHistoryKeepLast = 0; -constexpr uint32_t kReliabilityBestEffort = 1; -constexpr uint32_t kReliabilityReliable = 2; -constexpr uint32_t kDurabilityVolatile = 0; -constexpr uint32_t kLivelinessAutomatic = 0; -constexpr int32_t kDefaultLeaseDurationSeconds = 20; -constexpr uint32_t kDefaultLeaseDurationNanoseconds = 0; -constexpr int32_t kDefaultMaxBlockingSeconds = 0; -constexpr uint32_t kDefaultMaxBlockingNanoseconds = 100000000; -// PID_TYPE_MAX_SIZE_SERIALIZED carries the max CDR-serialized size of the type *including* the -// 4-byte encapsulation header (matching FastDDS: getMaxCdrSerializedSize() + 4). A UInt32 body is -// 4 bytes, so the spec-exact advertised value is 4 + 4 = 8. -constexpr uint32_t kUInt32SerializedSize = 8; - -enum class ParameterId : uint16_t { - PID_SENTINEL = 0x0001, - PID_PARTICIPANT_LEASE_DURATION = 0x0002, - PID_TOPIC_NAME = 0x0005, - PID_TYPE_NAME = 0x0007, - PID_DOMAIN_ID = 0x000f, - PID_PROTOCOL_VERSION = 0x0015, - PID_VENDORID = 0x0016, - PID_DURABILITY = 0x001d, - PID_RELIABILITY = 0x001a, - PID_LIVELINESS = 0x001b, - PID_USER_DATA = 0x002c, - PID_UNICAST_LOCATOR = 0x002f, - PID_DEFAULT_UNICAST_LOCATOR = 0x0031, - PID_METATRAFFIC_UNICAST_LOCATOR = 0x0032, - PID_METATRAFFIC_MULTICAST_LOCATOR = 0x0033, - PID_MULTICAST_LOCATOR = 0x0030, - PID_EXPECTS_INLINE_QOS = 0x0043, - PID_DEFAULT_MULTICAST_LOCATOR = 0x0048, - PID_PARTICIPANT_GUID = 0x0050, - PID_BUILTIN_ENDPOINT_SET = 0x0058, - PID_ENDPOINT_GUID = 0x005a, - PID_TYPE_MAX_SIZE_SERIALIZED = 0x0060, - PID_ENTITY_NAME = 0x0062, - PID_KEY_HASH = 0x0070, - PID_HISTORY = 0x0040, -}; - -// 64-bit FNV-1a hash. Used to derive the node-name portion of the GUID prefix with a full 64 bits -// of entropy regardless of the platform's size_t width. std::hash is only 32-bit on -// the 32-bit ESP32, which made bytes 8..11 of the prefix a repeated copy of bytes 4..7 (the shift -// `hash >> (8 * i)` for i >= 4 was undefined behavior on a 32-bit value). -uint64_t fnv1a_64(std::string_view text) { - uint64_t hash = 1469598103934665603ull; // FNV-1a 64-bit offset basis - for (unsigned char c : text) { - hash ^= c; - hash *= 1099511628211ull; // FNV-1a 64-bit prime - } - return hash; -} - -class ByteWriter { -public: - void append_bytes(std::span bytes) { - data_.insert(data_.end(), bytes.begin(), bytes.end()); - } - - template void append_bytes(const std::array &bytes) { - data_.insert(data_.end(), bytes.begin(), bytes.end()); - } - - template void append_chars(const std::array &bytes) { - data_.insert(data_.end(), bytes.begin(), bytes.end()); - } - - void append_u8(uint8_t value) { data_.push_back(value); } - - void append_u16_le(uint16_t value) { - data_.push_back(static_cast(value & 0xff)); - data_.push_back(static_cast((value >> 8) & 0xff)); - } - - void append_u32_le(uint32_t value) { - for (int i = 0; i < 4; i++) { - data_.push_back(static_cast((value >> (8 * i)) & 0xff)); - } - } - - void append_i32_le(int32_t value) { append_u32_le(static_cast(value)); } - - void append_sequence_number_le(int64_t value) { - auto high = static_cast(value >> 32); - auto low = static_cast(value & 0xffffffffu); - append_i32_le(high); - append_u32_le(low); - } - - size_t size() const { return data_.size(); } - - void align(size_t alignment) { - while (data_.size() % alignment != 0) { - data_.push_back(0); - } - } - - std::vector take() { return std::move(data_); } - -private: - std::vector data_; -}; - -class ByteReader { -public: - explicit ByteReader(std::span data) - : data_(data) {} - - bool read_u8(uint8_t &value) { - if (remaining() < 1) { - return false; - } - value = data_[offset_++]; - return true; - } - - bool read_u16_le(uint16_t &value) { - if (remaining() < 2) { - return false; - } - value = static_cast(data_[offset_]) | - static_cast(static_cast(data_[offset_ + 1]) << 8); - offset_ += 2; - return true; - } - - bool read_u16_be(uint16_t &value) { - if (remaining() < 2) { - return false; - } - value = static_cast(static_cast(data_[offset_]) << 8) | - static_cast(data_[offset_ + 1]); - offset_ += 2; - return true; - } - - bool read_u16(uint16_t &value, bool little_endian) { - return little_endian ? read_u16_le(value) : read_u16_be(value); - } - - bool read_u32_le(uint32_t &value) { - if (remaining() < 4) { - return false; - } - value = static_cast(data_[offset_]) | - (static_cast(data_[offset_ + 1]) << 8) | - (static_cast(data_[offset_ + 2]) << 16) | - (static_cast(data_[offset_ + 3]) << 24); - offset_ += 4; - return true; - } - - bool read_i32_le(int32_t &value) { - uint32_t unsigned_value = 0; - if (!read_u32_le(unsigned_value)) { - return false; - } - value = static_cast(unsigned_value); - return true; - } - - bool read_u32_be(uint32_t &value) { - if (remaining() < 4) { - return false; - } - value = (static_cast(data_[offset_]) << 24) | - (static_cast(data_[offset_ + 1]) << 16) | - (static_cast(data_[offset_ + 2]) << 8) | - static_cast(data_[offset_ + 3]); - offset_ += 4; - return true; - } - - bool read_u32(uint32_t &value, bool little_endian) { - return little_endian ? read_u32_le(value) : read_u32_be(value); - } - - bool read_sequence_number(int64_t &value, bool little_endian) { - uint32_t high = 0; - uint32_t low = 0; - if (little_endian) { - if (!read_u32_le(high) || !read_u32_le(low)) { - return false; - } - } else { - if (!read_u32_be(high) || !read_u32_be(low)) { - return false; - } - } - value = (static_cast(static_cast(high)) << 32) | low; - return true; - } - - bool skip(size_t length) { - if (remaining() < length) { - return false; - } - offset_ += length; - return true; - } - - bool read_bytes(std::span destination) { - if (remaining() < destination.size()) { - return false; - } - std::memcpy(destination.data(), data_.data() + offset_, destination.size()); - offset_ += destination.size(); - return true; - } - - std::span read_span(size_t length) { - if (remaining() < length) { - return {}; - } - auto span = data_.subspan(offset_, length); - offset_ += length; - return span; - } - - size_t remaining() const { return data_.size() - offset_; } - -private: - std::span data_; - size_t offset_{0}; -}; - -struct ParameterView { - ParameterId id{ParameterId::PID_SENTINEL}; - std::span value{}; -}; - -struct DataSubmessageView { - espp::RtpsParticipant::EntityId reader_id{}; - espp::RtpsParticipant::EntityId writer_id{}; - int64_t writer_sn{0}; - std::span serialized_payload{}; - bool inline_qos_present{false}; - bool data_present{false}; -}; - -std::string hex_string(std::span bytes) { - std::ostringstream stream; - stream << std::hex << std::setfill('0'); - for (size_t i = 0; i < bytes.size(); i++) { - if (i != 0) { - stream << ':'; - } - stream << std::setw(2) << static_cast(bytes[i]); - } - return stream.str(); -} - -bool parse_ipv4(std::string_view address, std::array &octets) { - std::array parsed{}; - size_t part_index = 0; - size_t cursor = 0; - while (cursor < address.size() && part_index < parsed.size()) { - auto next = address.find('.', cursor); - if (next == std::string_view::npos) { - next = address.size(); - } - if (next == cursor) { - return false; - } - unsigned value = 0; - for (size_t i = cursor; i < next; i++) { - if (!std::isdigit(static_cast(address[i]))) { - return false; - } - value = value * 10 + static_cast(address[i] - '0'); - if (value > 255) { - return false; - } - } - parsed[part_index++] = static_cast(value); - cursor = next + 1; - } - if (part_index != parsed.size() || cursor < address.size()) { - return false; - } - octets = parsed; - return true; -} - -void append_parameter_header(ByteWriter &writer, ParameterId id, uint16_t length) { - writer.append_u16_le(static_cast(id)); - writer.append_u16_le(length); -} - -void append_parameter_guid(ByteWriter &writer, ParameterId id, - const espp::RtpsParticipant::Guid &guid) { - append_parameter_header(writer, id, 16); - writer.append_bytes(guid.prefix.value); - writer.append_bytes(guid.entity_id.value); -} - -void append_parameter_protocol_version(ByteWriter &writer, - const espp::RtpsParticipant::ProtocolVersion &version) { - append_parameter_header(writer, ParameterId::PID_PROTOCOL_VERSION, 4); - writer.append_u8(version.major); - writer.append_u8(version.minor); - writer.append_u8(0); - writer.append_u8(0); -} - -void append_parameter_vendor_id(ByteWriter &writer, - const espp::RtpsParticipant::VendorId &vendor_id) { - append_parameter_header(writer, ParameterId::PID_VENDORID, 4); - writer.append_bytes(vendor_id.value); - writer.append_u8(0); - writer.append_u8(0); -} - -void append_parameter_u32(ByteWriter &writer, ParameterId id, uint32_t value) { - append_parameter_header(writer, id, 4); - writer.append_u32_le(value); -} - -void append_parameter_bool(ByteWriter &writer, ParameterId id, bool value) { - append_parameter_header(writer, id, 4); - writer.append_u8(value ? 1 : 0); - writer.append_u8(0); - writer.append_u8(0); - writer.append_u8(0); -} - -// RTPS Duration_t/Time_t use the NTP representation {int32 seconds, uint32 fraction} where the -// fraction is in units of 1/2^32 of a second (see DDSI-RTPS; OpenDDS RtpsCore.idl references RFC -// 1305). Convert a nanosecond count to that fraction so durations are encoded spec-exactly. -constexpr uint32_t ntp_fraction_from_nanoseconds(uint32_t nanoseconds) { - return static_cast((static_cast(nanoseconds) << 32) / 1000000000ULL); -} - -void append_parameter_duration(ByteWriter &writer, ParameterId id, int32_t seconds, - uint32_t nanoseconds) { - append_parameter_header(writer, id, 8); - writer.append_i32_le(seconds); - writer.append_u32_le(ntp_fraction_from_nanoseconds(nanoseconds)); -} - -void append_parameter_locator(ByteWriter &writer, ParameterId id, - const espp::RtpsParticipant::Locator &locator) { - append_parameter_header(writer, id, 24); - // Locator_t.kind and .port are CDR long/unsigned long encoded in the parameter list endianness - // (little-endian for PL_CDR_LE); only the 16-byte address is a raw per-byte (network-order) - // field. - writer.append_u32_le(static_cast(locator.kind)); - writer.append_u32_le(locator.port); - writer.append_bytes(locator.address); -} - -// Append a CDR-encoded parameter value, padding it to the 4-byte multiple the RTPS PL_CDR -// encoding requires (parameterLength declares the padded length so the next parameter starts -// 4-byte aligned). -void append_cdr_parameter(ByteWriter &writer, ParameterId id, std::span body) { - const size_t padded = (body.size() + 3) & ~size_t{3}; - if (padded > 0xffff) { - // parameterLength is a 16-bit field; dropping an oversized parameter keeps the list - // well-formed on the wire (discovery parameters are tiny in practice). - return; - } - append_parameter_header(writer, id, static_cast(padded)); - writer.append_bytes( - std::span(reinterpret_cast(body.data()), body.size())); - for (size_t i = body.size(); i < padded; ++i) { - writer.append_u8(0); - } -} - -void append_parameter_string_cdr(ByteWriter &writer, ParameterId id, std::string_view text) { - auto body = cdr::serialize_body(std::string(text)); - if (!body) { - return; // drop the parameter on serialization failure; keeps the list well-formed - } - append_cdr_parameter(writer, id, *body); -} - -void append_parameter_octet_sequence(ByteWriter &writer, ParameterId id, - std::span bytes) { - auto body = cdr::serialize_body(std::vector(bytes.begin(), bytes.end())); - if (!body) { - return; // drop the parameter on serialization failure; keeps the list well-formed - } - append_cdr_parameter(writer, id, *body); -} - -void append_parameter_reliability(ByteWriter &writer, - espp::RtpsParticipant::ReliabilityKind reliability) { - append_parameter_header(writer, ParameterId::PID_RELIABILITY, 12); - writer.append_u32_le(reliability == espp::RtpsParticipant::ReliabilityKind::RELIABLE - ? kReliabilityReliable - : kReliabilityBestEffort); - writer.append_i32_le(kDefaultMaxBlockingSeconds); - writer.append_u32_le(ntp_fraction_from_nanoseconds(kDefaultMaxBlockingNanoseconds)); -} - -void append_parameter_durability(ByteWriter &writer) { - append_parameter_header(writer, ParameterId::PID_DURABILITY, 4); - writer.append_u32_le(kDurabilityVolatile); -} - -void append_parameter_liveliness(ByteWriter &writer) { - append_parameter_header(writer, ParameterId::PID_LIVELINESS, 12); - writer.append_u32_le(kLivelinessAutomatic); - writer.append_i32_le(kDefaultLeaseDurationSeconds); - writer.append_u32_le(ntp_fraction_from_nanoseconds(kDefaultLeaseDurationNanoseconds)); -} - -void append_parameter_history(ByteWriter &writer, uint32_t depth = 1) { - append_parameter_header(writer, ParameterId::PID_HISTORY, 8); - writer.append_u32_le(kHistoryKeepLast); // history kind: KEEP_LAST - writer.append_u32_le(depth); // history depth -} - -void append_parameter_key_hash(ByteWriter &writer, const espp::RtpsParticipant::Guid &guid) { - append_parameter_header(writer, ParameterId::PID_KEY_HASH, 16); - writer.append_bytes(guid.prefix.value); - writer.append_bytes(guid.entity_id.value); -} - -void append_parameter_sentinel(ByteWriter &writer) { - append_parameter_header(writer, ParameterId::PID_SENTINEL, 0); -} - -std::vector parse_parameter_list(std::span payload) { - std::vector parameters; - // Limitation: only little-endian parameter lists (PL_CDR_LE, encapsulation id 0x0003) are - // decoded. The parameter value parsers below (parse_u32_le, parse_locator, parse_guid, ...) - // assume little-endian contents, so a big-endian (PL_CDR_BE) list is intentionally rejected - // rather than misparsed. In practice DDS and ROS 2 implementations emit PL_CDR_LE for SPDP/SEDP - // discovery, so this is a discovery-only gap. - if (payload.size() < 4 || payload[0] != 0x00 || payload[1] != 0x03) { - return parameters; - } - - ByteReader reader(payload.subspan(4)); - while (reader.remaining() >= 4) { - uint16_t pid = 0; - uint16_t length = 0; - if (!reader.read_u16_le(pid) || !reader.read_u16_le(length)) { - return {}; - } - if (pid == static_cast(ParameterId::PID_SENTINEL)) { - break; - } - auto value = reader.read_span(length); - if (value.size() != length) { - return {}; - } - parameters.push_back({.id = static_cast(pid), .value = value}); - auto padding = (4 - (length % 4)) & 0x3; - if (padding > 0 && reader.read_span(padding).size() != padding) { - return {}; - } - } - return parameters; -} - -std::optional find_parameter(std::span parameters, - ParameterId id) { - auto iterator = std::find_if(parameters.begin(), parameters.end(), - [id](const auto ¶meter) { return parameter.id == id; }); - if (iterator == parameters.end()) { - return std::nullopt; - } - return *iterator; -} - -std::vector find_parameters(std::span parameters, - ParameterId id) { - std::vector matches; - std::copy_if(parameters.begin(), parameters.end(), std::back_inserter(matches), - [id](const auto ¶meter) { return parameter.id == id; }); - return matches; -} - -std::optional parse_guid(std::span value) { - if (value.size() != 16) { - return std::nullopt; - } - espp::RtpsParticipant::Guid guid; - std::memcpy(guid.prefix.value.data(), value.data(), guid.prefix.value.size()); - std::memcpy(guid.entity_id.value.data(), value.data() + guid.prefix.value.size(), - guid.entity_id.value.size()); - return guid; -} - -std::optional parse_u32_le(std::span value) { - ByteReader reader(value); - uint32_t parsed = 0; - if (!reader.read_u32_le(parsed)) { - return std::nullopt; - } - return parsed; -} - -std::optional parse_bool(std::span value) { - if (value.size() < 1) { - return std::nullopt; - } - return value[0] != 0; -} - -std::optional parse_cdr_string(std::span value) { - auto result = cdr::deserialize_body(std::as_bytes(value)); - if (!result) { - return std::nullopt; - } - return *std::move(result); -} - -std::optional> parse_octet_sequence(std::span value) { - auto result = cdr::deserialize_body>(std::as_bytes(value)); - if (!result) { - return std::nullopt; - } - return *std::move(result); -} - -std::optional parse_locator(std::span value) { - if (value.size() != 24) { - return std::nullopt; - } - ByteReader reader(value); - uint32_t kind = 0; - uint32_t port = 0; - espp::RtpsParticipant::Locator locator; - // kind and port are little-endian in PL_CDR_LE (see append_parameter_locator); the address is a - // raw 16-byte field read verbatim. - if (!reader.read_u32_le(kind) || !reader.read_u32_le(port) || - !reader.read_bytes(std::span{locator.address.data(), locator.address.size()})) { - return std::nullopt; - } - locator.kind = static_cast(static_cast(kind)); - locator.port = port; - return locator; -} - -bool has_valid_locator(const espp::RtpsParticipant::Locator &locator) { - return locator.kind == espp::RtpsParticipant::Locator::Kind::UDP_V4 && locator.port != 0 && - std::any_of(locator.address.begin() + 12, locator.address.end(), - [](uint8_t octet) { return octet != 0; }); -} - -// Human-readable "kind/address:port" for a locator, for discovery logging. -std::string locator_to_string(const espp::RtpsParticipant::Locator &locator) { - if (locator.kind == espp::RtpsParticipant::Locator::Kind::INVALID) { - return ""; - } - return fmt::format("udpv4/{}:{}", locator.address_string(), locator.port); -} - -// Same as above for an optional locator that may not have been present in a message. -std::string locator_to_string(const std::optional &locator) { - return locator ? locator_to_string(*locator) : ""; -} - -std::optional -parse_reliability(std::span value) { - auto maybe_kind = parse_u32_le(value); - if (!maybe_kind) { - return std::nullopt; - } - if (*maybe_kind == kReliabilityReliable) { - return espp::RtpsParticipant::ReliabilityKind::RELIABLE; - } - return espp::RtpsParticipant::ReliabilityKind::BEST_EFFORT; -} - -std::string extract_enclave(std::span user_data_bytes) { - std::string text(reinterpret_cast(user_data_bytes.data()), user_data_bytes.size()); - std::string key = "enclave="; - auto position = text.find(key); - if (position == std::string::npos) { - return "/"; - } - position += key.size(); - auto end = text.find(';', position); - if (end == std::string::npos) { - end = text.size(); - } - // Normalize an empty enclave (e.g. "enclave=;") to the default "/" rather than returning "". - if (end == position) { - return "/"; - } - return text.substr(position, end - position); -} - -std::array entity_id_for_index(uint32_t entity_index, uint8_t kind) { - return {0x00, 0x00, static_cast(0x10 + entity_index), kind}; -} - -bool is_same_guid_prefix(const espp::RtpsParticipant::Guid &guid, - const espp::RtpsParticipant::GuidPrefix &prefix) { - return guid.prefix == prefix; -} - -// Skip an inline-QoS parameter list (a raw ParameterList without an encapsulation header) up to and -// including its PID_SENTINEL terminator. Returns false if the list is malformed/truncated. -bool skip_inline_qos(ByteReader &reader, bool little_endian) { - while (reader.remaining() >= 4) { - uint16_t pid = 0; - uint16_t length = 0; - if (!reader.read_u16(pid, little_endian) || !reader.read_u16(length, little_endian)) { - return false; - } - if (pid == static_cast(ParameterId::PID_SENTINEL)) { - return true; - } - if (!reader.skip(length)) { - return false; - } - } - return false; -} - -DataSubmessageView parse_data_submessage(const espp::RtpsParticipant::Submessage &submessage, - bool &ok) { - DataSubmessageView view; - ok = false; - if (submessage.kind != espp::RtpsParticipant::SubmessageKind::DATA || - (submessage.flags & kSubmessageFlagData) == 0) { - return view; - } - - const bool little_endian = (submessage.flags & kSubmessageFlagLittleEndian) != 0; - ByteReader reader(std::span{submessage.payload.data(), submessage.payload.size()}); - uint16_t extra_flags = 0; - uint16_t octets_to_inline_qos = 0; - if (!reader.read_u16(extra_flags, little_endian) || - !reader.read_u16(octets_to_inline_qos, little_endian) || - !reader.read_bytes( - std::span{view.reader_id.value.data(), view.reader_id.value.size()}) || - !reader.read_bytes( - std::span{view.writer_id.value.data(), view.writer_id.value.size()}) || - !reader.read_sequence_number(view.writer_sn, little_endian)) { - return view; - } - - view.inline_qos_present = (submessage.flags & kSubmessageFlagInlineQos) != 0; - view.data_present = true; - - // octetsToInlineQos counts from the byte after the octetsToInlineQos field to the start of the - // inline QoS (or the serialized payload when no inline QoS is present). We have already consumed - // the standard 16-byte readerId+writerId+writerSN block; honor any additional header octets a - // sender may have included instead of assuming the fixed layout. - if (octets_to_inline_qos < kDataSubmessageOctetsToInlineQos || - !reader.skip(octets_to_inline_qos - kDataSubmessageOctetsToInlineQos)) { - return view; - } - - // When inline QoS is present, skip past the inline QoS parameter list to reach the serialized - // payload rather than dropping the sample. - if (view.inline_qos_present && !skip_inline_qos(reader, little_endian)) { - return view; - } - - view.serialized_payload = reader.read_span(reader.remaining()); - ok = true; - return view; -} - -std::vector build_parameter_list_payload(ByteWriter ¶meter_writer) { - auto parameter_bytes = parameter_writer.take(); - // PL_CDR_LE encapsulation header (representation id 0x0003, big-endian on the wire) followed by - // the parameter list. - std::vector payload; - payload.reserve(4 + parameter_bytes.size()); - payload.insert(payload.end(), {0x00, 0x03, 0x00, 0x00}); - payload.insert(payload.end(), parameter_bytes.begin(), parameter_bytes.end()); - return payload; -} - -std::vector build_data_submessage_payload(const espp::RtpsParticipant::EntityId &reader_id, - const espp::RtpsParticipant::EntityId &writer_id, - int64_t sequence_number, - std::span serialized_payload) { - ByteWriter writer; - writer.append_u16_le(0); - writer.append_u16_le(kDataSubmessageOctetsToInlineQos); - writer.append_bytes(reader_id.value); - writer.append_bytes(writer_id.value); - writer.append_sequence_number_le(sequence_number); - writer.append_bytes(serialized_payload); - writer.align(4); - return writer.take(); -} - -espp::RtpsParticipant::Message build_message(const espp::RtpsParticipant::GuidPrefix &guid_prefix, - const espp::RtpsParticipant::EntityId &reader_id, - const espp::RtpsParticipant::EntityId &writer_id, - int64_t sequence_number, - std::span serialized_payload) { - return {.header = {.guid_prefix = guid_prefix}, - .submessages = {{ - .kind = espp::RtpsParticipant::SubmessageKind::DATA, - .flags = static_cast(kSubmessageFlagLittleEndian | kSubmessageFlagData), - .payload = build_data_submessage_payload(reader_id, writer_id, sequence_number, - serialized_payload), - }}}; -} - -// --------------------------------------------------------------------------- -// Reliable QoS submessage codecs (HEARTBEAT / ACKNACK / INFO_DST). See -// RELIABLE_RTPS_PLAN.md. These are the Phase 0 wire-format primitives; they are -// wired into the writer/reader state machines in later phases. -// --------------------------------------------------------------------------- - -// RTPS SequenceNumberSet: a bitmapBase plus up to 256 bits marking which of -// [bitmapBase, bitmapBase + numBits) sequence numbers are present in the set -// (used by ACKNACK to mark the missing/requested sequence numbers). Bit i is the -// (31 - i % 32)-th bit (MSB-first) of word (i / 32). -struct SequenceNumberSet { - static constexpr uint32_t kMaxBits = 256; - int64_t base{1}; ///< bitmapBase: lowest sequence number the set can represent. - uint32_t num_bits{0}; ///< Number of valid bits (0..256). - std::array bitmap{}; ///< Up to 256 bits (8 x uint32), MSB-first within each word. - - uint32_t num_words() const { return (num_bits + 31) / 32; } - - // Mark a sequence number as present in the set (no-op if out of [base, base+256)). - void set(int64_t sequence_number) { - if (sequence_number < base) { - return; - } - int64_t delta = sequence_number - base; - if (delta >= static_cast(kMaxBits)) { - return; - } - auto index = static_cast(delta); - num_bits = std::max(num_bits, index + 1); - bitmap[index / 32] |= (1u << (31 - (index % 32))); - } - - bool contains(int64_t sequence_number) const { - if (sequence_number < base) { - return false; - } - int64_t delta = sequence_number - base; - if (delta >= static_cast(num_bits)) { - return false; - } - auto index = static_cast(delta); - return (bitmap[index / 32] >> (31 - (index % 32))) & 1u; - } -}; - -void append_sequence_number_set(ByteWriter &writer, const SequenceNumberSet &set) { - writer.append_sequence_number_le(set.base); - writer.append_u32_le(set.num_bits); - for (uint32_t word = 0; word < set.num_words(); word++) { - writer.append_u32_le(set.bitmap[word]); - } -} - -bool read_sequence_number_set(ByteReader &reader, bool little_endian, SequenceNumberSet &set) { - set = SequenceNumberSet{}; - if (!reader.read_sequence_number(set.base, little_endian) || - !reader.read_u32(set.num_bits, little_endian)) { - return false; - } - if (set.num_bits > SequenceNumberSet::kMaxBits) { - return false; - } - for (uint32_t word = 0; word < set.num_words(); word++) { - if (!reader.read_u32(set.bitmap[word], little_endian)) { - return false; - } - } - return true; -} - -std::vector build_info_dst_payload(const espp::RtpsParticipant::GuidPrefix &dest_prefix) { - ByteWriter writer; - writer.append_bytes(dest_prefix.value); - return writer.take(); -} - -espp::RtpsParticipant::Submessage -build_info_dst_submessage(const espp::RtpsParticipant::GuidPrefix &dest_prefix) { - return {.kind = espp::RtpsParticipant::SubmessageKind::INFO_DST, - .flags = kSubmessageFlagLittleEndian, - .payload = build_info_dst_payload(dest_prefix)}; -} - -std::vector build_heartbeat_payload(const espp::RtpsParticipant::EntityId &reader_id, - const espp::RtpsParticipant::EntityId &writer_id, - int64_t first_sn, int64_t last_sn, uint32_t count) { - ByteWriter writer; - writer.append_bytes(reader_id.value); - writer.append_bytes(writer_id.value); - writer.append_sequence_number_le(first_sn); - writer.append_sequence_number_le(last_sn); - writer.append_u32_le(count); - return writer.take(); -} - -espp::RtpsParticipant::Submessage -build_heartbeat_submessage(const espp::RtpsParticipant::EntityId &reader_id, - const espp::RtpsParticipant::EntityId &writer_id, int64_t first_sn, - int64_t last_sn, uint32_t count, bool final) { - uint8_t flags = kSubmessageFlagLittleEndian | (final ? kSubmessageFlagFinal : 0); - return {.kind = espp::RtpsParticipant::SubmessageKind::HEARTBEAT, - .flags = flags, - .payload = build_heartbeat_payload(reader_id, writer_id, first_sn, last_sn, count)}; -} - -std::vector build_acknack_payload(const espp::RtpsParticipant::EntityId &reader_id, - const espp::RtpsParticipant::EntityId &writer_id, - const SequenceNumberSet &reader_sn_state, - uint32_t count) { - ByteWriter writer; - writer.append_bytes(reader_id.value); - writer.append_bytes(writer_id.value); - append_sequence_number_set(writer, reader_sn_state); - writer.append_u32_le(count); - return writer.take(); -} - -espp::RtpsParticipant::Submessage -build_acknack_submessage(const espp::RtpsParticipant::EntityId &reader_id, - const espp::RtpsParticipant::EntityId &writer_id, - const SequenceNumberSet &reader_sn_state, uint32_t count, bool final) { - uint8_t flags = kSubmessageFlagLittleEndian | (final ? kSubmessageFlagFinal : 0); - return {.kind = espp::RtpsParticipant::SubmessageKind::ACKNACK, - .flags = flags, - .payload = build_acknack_payload(reader_id, writer_id, reader_sn_state, count)}; -} - -struct HeartbeatView { - espp::RtpsParticipant::EntityId reader_id{}; - espp::RtpsParticipant::EntityId writer_id{}; - int64_t first_sn{0}; - int64_t last_sn{0}; - uint32_t count{0}; - bool final{false}; - bool liveliness{false}; - bool valid{false}; -}; - -HeartbeatView parse_heartbeat_submessage(const espp::RtpsParticipant::Submessage &submessage) { - HeartbeatView view; - if (submessage.kind != espp::RtpsParticipant::SubmessageKind::HEARTBEAT) { - return view; - } - const bool little_endian = (submessage.flags & kSubmessageFlagLittleEndian) != 0; - ByteReader reader(std::span{submessage.payload.data(), submessage.payload.size()}); - if (!reader.read_bytes( - std::span{view.reader_id.value.data(), view.reader_id.value.size()}) || - !reader.read_bytes( - std::span{view.writer_id.value.data(), view.writer_id.value.size()}) || - !reader.read_sequence_number(view.first_sn, little_endian) || - !reader.read_sequence_number(view.last_sn, little_endian) || - !reader.read_u32(view.count, little_endian)) { - return view; - } - view.final = (submessage.flags & kSubmessageFlagFinal) != 0; - view.liveliness = (submessage.flags & kSubmessageFlagLiveliness) != 0; - view.valid = true; - return view; -} - -struct AckNackView { - espp::RtpsParticipant::EntityId reader_id{}; - espp::RtpsParticipant::EntityId writer_id{}; - SequenceNumberSet reader_sn_state{}; - uint32_t count{0}; - bool final{false}; - bool valid{false}; -}; - -AckNackView parse_acknack_submessage(const espp::RtpsParticipant::Submessage &submessage) { - AckNackView view; - if (submessage.kind != espp::RtpsParticipant::SubmessageKind::ACKNACK) { - return view; - } - const bool little_endian = (submessage.flags & kSubmessageFlagLittleEndian) != 0; - ByteReader reader(std::span{submessage.payload.data(), submessage.payload.size()}); - if (!reader.read_bytes( - std::span{view.reader_id.value.data(), view.reader_id.value.size()}) || - !reader.read_bytes( - std::span{view.writer_id.value.data(), view.writer_id.value.size()}) || - !read_sequence_number_set(reader, little_endian, view.reader_sn_state) || - !reader.read_u32(view.count, little_endian)) { - return view; - } - view.final = (submessage.flags & kSubmessageFlagFinal) != 0; - view.valid = true; - return view; -} - -// Return the sequence numbers a reader is requesting (the set bits) from an ACKNACK's reader SN -// state. A positive ack (empty set) yields an empty list. -std::vector requested_sequence_numbers(const SequenceNumberSet &set) { - std::vector sequence_numbers; - for (uint32_t bit = 0; bit < set.num_bits; bit++) { - const int64_t sequence_number = set.base + static_cast(bit); - if (set.contains(sequence_number)) { - sequence_numbers.push_back(sequence_number); - } - } - return sequence_numbers; -} - -// GAP submessage: readerId(4) writerId(4) gapStart:SN(8) gapList:SequenceNumberSet. The samples in -// [gapStart, gapList.base - 1] plus the set bits in gapList are irrelevant / no longer available. -std::vector build_gap_payload(const espp::RtpsParticipant::EntityId &reader_id, - const espp::RtpsParticipant::EntityId &writer_id, - int64_t gap_start, const SequenceNumberSet &gap_list) { - ByteWriter writer; - writer.append_bytes(reader_id.value); - writer.append_bytes(writer_id.value); - writer.append_sequence_number_le(gap_start); - append_sequence_number_set(writer, gap_list); - return writer.take(); -} - -espp::RtpsParticipant::Submessage -build_gap_submessage(const espp::RtpsParticipant::EntityId &reader_id, - const espp::RtpsParticipant::EntityId &writer_id, int64_t gap_start, - const SequenceNumberSet &gap_list) { - return {.kind = espp::RtpsParticipant::SubmessageKind::GAP, - .flags = kSubmessageFlagLittleEndian, - .payload = build_gap_payload(reader_id, writer_id, gap_start, gap_list)}; -} - -struct GapView { - espp::RtpsParticipant::EntityId reader_id{}; - espp::RtpsParticipant::EntityId writer_id{}; - int64_t gap_start{0}; - SequenceNumberSet gap_list{}; - bool valid{false}; -}; - -GapView parse_gap_submessage(const espp::RtpsParticipant::Submessage &submessage) { - GapView view; - if (submessage.kind != espp::RtpsParticipant::SubmessageKind::GAP) { - return view; - } - const bool little_endian = (submessage.flags & kSubmessageFlagLittleEndian) != 0; - ByteReader reader(std::span{submessage.payload.data(), submessage.payload.size()}); - if (!reader.read_bytes( - std::span{view.reader_id.value.data(), view.reader_id.value.size()}) || - !reader.read_bytes( - std::span{view.writer_id.value.data(), view.writer_id.value.size()}) || - !reader.read_sequence_number(view.gap_start, little_endian) || - !read_sequence_number_set(reader, little_endian, view.gap_list)) { - return view; - } - view.valid = true; - return view; -} -} // namespace - -namespace espp { -std::string RtpsParticipant::GuidPrefix::to_string() const { return hex_string(value); } - -std::string RtpsParticipant::EntityId::to_string() const { return hex_string(value); } - -std::string RtpsParticipant::Guid::to_string() const { - return prefix.to_string() + '|' + entity_id.to_string(); -} - -RtpsParticipant::Locator RtpsParticipant::Locator::udp_v4(std::string_view ipv4_address, - uint16_t port) { - Locator locator; - locator.kind = Kind::UDP_V4; - locator.port = port; - std::array octets{}; - if (parse_ipv4(ipv4_address, octets)) { - locator.address[12] = octets[0]; - locator.address[13] = octets[1]; - locator.address[14] = octets[2]; - locator.address[15] = octets[3]; - } - return locator; -} - -std::string RtpsParticipant::Locator::address_string() const { - if (kind != Kind::UDP_V4) { - return "0.0.0.0"; - } - std::ostringstream stream; - stream << static_cast(address[12]) << '.' << static_cast(address[13]) << '.' - << static_cast(address[14]) << '.' << static_cast(address[15]); - return stream.str(); -} - -std::vector RtpsParticipant::Message::serialize() const { - ByteWriter writer; - writer.append_chars(kRtpsMagic); - writer.append_u8(header.protocol_version.major); - writer.append_u8(header.protocol_version.minor); - writer.append_bytes(header.vendor_id.value); - writer.append_bytes(header.guid_prefix.value); - for (const auto &submessage : submessages) { - writer.append_u8(static_cast(submessage.kind)); - writer.append_u8(submessage.flags); - writer.append_u16_le(static_cast(submessage.payload.size())); - writer.append_bytes(submessage.payload); - } - return writer.take(); -} - -std::optional -RtpsParticipant::Message::parse(std::span data) { - if (data.size() < 20 || !std::equal(kRtpsMagic.begin(), kRtpsMagic.end(), data.begin())) { - return std::nullopt; - } - - ByteReader reader(data.subspan(4)); - Message message; - if (!reader.read_u8(message.header.protocol_version.major) || - !reader.read_u8(message.header.protocol_version.minor) || - !reader.read_bytes(std::span{message.header.vendor_id.value.data(), - message.header.vendor_id.value.size()}) || - !reader.read_bytes(std::span{message.header.guid_prefix.value.data(), - message.header.guid_prefix.value.size()})) { - return std::nullopt; - } - - while (reader.remaining() > 0) { - Submessage submessage; - uint8_t kind = 0; - uint16_t length = 0; - // Limitation: submessageLength is read little-endian regardless of the submessage E-flag (bit 0 - // of flags). Big-endian submessages are not supported; in practice DDS/ROS 2 peers emit - // little-endian framing. Endianness of the DATA submessage body itself is honored separately in - // parse_data_submessage(). - if (!reader.read_u8(kind) || !reader.read_u8(submessage.flags) || !reader.read_u16_le(length)) { - return std::nullopt; - } - auto payload = reader.read_span(length); - if (payload.size() != length) { - return std::nullopt; - } - submessage.kind = static_cast(kind); - submessage.payload.assign(payload.begin(), payload.end()); - message.submessages.push_back(std::move(submessage)); - } - return message; -} - -RtpsParticipant::RtpsParticipant(const Config &config) - : BaseComponent({.tag = "RtpsParticipant", .level = config.log_level}) - , config_(config) { - // GUID prefix layout: bytes 0..1 = participant_id, 2..3 = domain_id, 4..11 = 64-bit node-name - // hash. Uniqueness across participants on one host relies on distinct participant_ids; the - // node-name hash distinguishes different nodes/applications. By default we also mix per-instance - // entropy into the hash so a restarted participant presents a new GUID (otherwise reliable peers - // drop its republished samples as already-seen duplicates). - uint64_t hash = fnv1a_64(config_.node_name); - if (config_.randomize_guid_prefix) { - hash ^= random_guid_entropy(); - } - guid_prefix_.value[0] = config_.participant_id & 0xff; - guid_prefix_.value[1] = (config_.participant_id >> 8) & 0xff; - guid_prefix_.value[2] = config_.domain_id & 0xff; - guid_prefix_.value[3] = (config_.domain_id >> 8) & 0xff; - for (size_t i = 0; i < 8; i++) { - guid_prefix_.value[4 + i] = static_cast((hash >> (8 * i)) & 0xff); - } -} - -RtpsParticipant::~RtpsParticipant() { stop(); } - -bool RtpsParticipant::start() { - if (started_.exchange(true)) { - return false; - } - - auto port_mapping = ports(); - logger_.info("RTPS participant {} starting: node '{}', domain {}, pid {}, bind {}, advertised {} " - "| ports meta_mc={} meta_uc={} user_mc={} user_uc={} | meta_mc_group {}", - guid_prefix_.to_string(), config_.node_name, config_.domain_id, - config_.participant_id, config_.bind_address, config_.advertised_address, - port_mapping.metatraffic_multicast, port_mapping.metatraffic_unicast, - port_mapping.user_multicast, port_mapping.user_unicast, - config_.metatraffic_multicast_group); - metatraffic_multicast_receiver_ = - std::make_unique(UdpSocket::Config{.log_level = config_.socket_log_level}); - metatraffic_unicast_receiver_ = - std::make_unique(UdpSocket::Config{.log_level = config_.socket_log_level}); - user_unicast_receiver_ = - std::make_unique(UdpSocket::Config{.log_level = config_.socket_log_level}); - - auto multicast_task_config = config_.receive_task_config; - multicast_task_config.name = config_.receive_task_config.name + "_spdp_mc"; - auto multicast_receive_config = UdpSocket::ReceiveConfig{ - .port = port_mapping.metatraffic_multicast, - .buffer_size = 4096, - .is_multicast_endpoint = true, - .multicast_group = config_.metatraffic_multicast_group, - .multicast_interface = config_.bind_address, - .on_receive_callback = [this](auto &data, - const auto &sender) -> std::optional> { - handle_metatraffic_message(data, sender); - return std::nullopt; - }, - }; - if (!metatraffic_multicast_receiver_->start_receiving(multicast_task_config, - multicast_receive_config)) { - logger_.error("Failed to start metatraffic multicast receiver"); - stop(); - return false; - } - - auto unicast_meta_task_config = config_.receive_task_config; - unicast_meta_task_config.name = config_.receive_task_config.name + "_meta_uc"; - auto unicast_meta_receive_config = UdpSocket::ReceiveConfig{ - .port = port_mapping.metatraffic_unicast, - .buffer_size = 4096, - .on_receive_callback = [this](auto &data, - const auto &sender) -> std::optional> { - handle_metatraffic_message(data, sender); - return std::nullopt; - }, - }; - if (!metatraffic_unicast_receiver_->start_receiving(unicast_meta_task_config, - unicast_meta_receive_config)) { - logger_.error("Failed to start metatraffic unicast receiver"); - stop(); - return false; - } - - auto user_task_config = config_.receive_task_config; - user_task_config.name = config_.receive_task_config.name + "_user_uc"; - auto user_receive_config = UdpSocket::ReceiveConfig{ - .port = port_mapping.user_unicast, - .buffer_size = 4096, - .on_receive_callback = [this](auto &data, - const auto &sender) -> std::optional> { - handle_user_message(data, sender); - return std::nullopt; - }, - }; - if (!user_unicast_receiver_->start_receiving(user_task_config, user_receive_config)) { - logger_.error("Failed to start user unicast receiver"); - stop(); - return false; - } - - if (!ensure_user_multicast_receivers_started()) { - stop(); - return false; - } - - announce_task_ = Task::make_unique({ - .callback = [this](std::mutex &mutex, std::condition_variable &cv, bool ¬ified) -> bool { - send_discovery_now(); - std::unique_lock lock(mutex); - auto stop_requested = - cv.wait_for(lock, config_.announce_period, [¬ified] { return notified; }); - notified = false; - return stop_requested; - }, - .task_config = config_.announce_task_config, - .log_level = get_log_level(), - }); - announce_task_->start(); - send_discovery_now(); - - // Periodic HEARTBEAT for reliable writers so matched reliable readers can detect gaps and catch - // up (e.g. late joiners). Does nothing while there are no reliable writers / cached samples. - heartbeat_task_ = Task::make_unique({ - .callback = [this](std::mutex &mutex, std::condition_variable &cv, bool ¬ified) -> bool { - send_heartbeats_now(); - std::unique_lock lock(mutex); - auto stop_requested = - cv.wait_for(lock, config_.heartbeat_period, [¬ified] { return notified; }); - notified = false; - return stop_requested; - }, - .task_config = config_.heartbeat_task_config, - .log_level = get_log_level(), - }); - heartbeat_task_->start(); - return true; -} - -void RtpsParticipant::stop() { - started_ = false; - if (announce_task_) { - announce_task_->stop(); - announce_task_.reset(); - } - if (heartbeat_task_) { - heartbeat_task_->stop(); - heartbeat_task_.reset(); - } - if (metatraffic_multicast_receiver_) { - metatraffic_multicast_receiver_->stop_receiving(); - metatraffic_multicast_receiver_.reset(); - } - if (metatraffic_unicast_receiver_) { - metatraffic_unicast_receiver_->stop_receiving(); - metatraffic_unicast_receiver_.reset(); - } - { - std::lock_guard receivers_lock(receivers_mutex_); - for (auto &receiver : user_multicast_receivers_) { - if (receiver.socket) { - receiver.socket->stop_receiving(); - } - } - user_multicast_receivers_.clear(); - } - if (user_unicast_receiver_) { - user_unicast_receiver_->stop_receiving(); - user_unicast_receiver_.reset(); - } - { - std::lock_guard lock(reliable_mutex_); - writer_reliable_states_.clear(); - reader_reliable_states_.clear(); - builtin_reader_states_.clear(); - } -} - -bool RtpsParticipant::is_started() const { return started_.load(); } - -bool RtpsParticipant::add_writer(const WriterConfig &writer_config) { - std::lock_guard lock(mutex_); - writers_.push_back(writer_config); - return true; -} - -bool RtpsParticipant::add_reader(const ReaderConfig &reader_config) { - // Bring up the reader's multicast receiver (if any) before persisting the reader, so a failure - // does not leave the participant with a registered reader that has no working receiver. - if (started_.load() && !reader_config.multicast_group.empty() && - !ensure_user_multicast_receivers_started(reader_config.multicast_group)) { - logger_.error("Failed to start multicast receiver for topic '{}'", reader_config.topic_name); - return false; - } - std::lock_guard lock(mutex_); - readers_.push_back(reader_config); - return true; -} - -size_t RtpsParticipant::GuidHash::operator()(const Guid &guid) const { - // FNV-1a over the 12-byte prefix + 4-byte entity id. - uint64_t hash = 1469598103934665603ull; - auto mix = [&hash](uint8_t byte) { - hash ^= byte; - hash *= 1099511628211ull; - }; - for (auto byte : guid.prefix.value) { - mix(byte); - } - for (auto byte : guid.entity_id.value) { - mix(byte); - } - return static_cast(hash); -} - -RtpsParticipant::DiscoveryDb::UpsertResult -RtpsParticipant::DiscoveryDb::upsert_participant( - const Guid &participant_guid, const std::function &apply) { - std::lock_guard lock(mutex_); - auto [iterator, inserted] = participants_.try_emplace(participant_guid); - apply(iterator->second); // merge only the fields present in this announcement - return {.is_new = inserted, .value = iterator->second}; -} - -RtpsParticipant::DiscoveryDb::UpsertResult -RtpsParticipant::DiscoveryDb::upsert_endpoint(bool is_reader, const Guid &endpoint_guid, - const std::function &apply) { - std::lock_guard lock(mutex_); - auto &endpoints = is_reader ? readers_ : writers_; - auto [iterator, inserted] = endpoints.try_emplace(endpoint_guid); - apply(iterator->second); // merge only the fields present in this announcement - return {.is_new = inserted, .value = iterator->second}; -} - -std::optional -RtpsParticipant::DiscoveryDb::find_participant_by_prefix(const GuidPrefix &prefix) const { - std::lock_guard lock(mutex_); - for (const auto &[guid, participant] : participants_) { - if (participant.guid_prefix == prefix) { - return participant; - } - } - return std::nullopt; -} - -std::optional -RtpsParticipant::DiscoveryDb::find_writer(const Guid &guid) const { - std::lock_guard lock(mutex_); - auto iterator = writers_.find(guid); - if (iterator == writers_.end()) { - return std::nullopt; - } - return iterator->second; -} - -std::vector RtpsParticipant::DiscoveryDb::participants() const { - std::lock_guard lock(mutex_); - std::vector result; - result.reserve(participants_.size()); - for (const auto &[guid, participant] : participants_) { - result.push_back(participant); - } - return result; -} - -std::vector RtpsParticipant::DiscoveryDb::writers() const { - std::lock_guard lock(mutex_); - std::vector result; - result.reserve(writers_.size()); - for (const auto &[guid, writer] : writers_) { - result.push_back(writer); - } - return result; -} - -std::vector RtpsParticipant::DiscoveryDb::readers() const { - std::lock_guard lock(mutex_); - std::vector result; - result.reserve(readers_.size()); - for (const auto &[guid, reader] : readers_) { - result.push_back(reader); - } - return result; -} - -void RtpsParticipant::DiscoveryDb::clear() { - std::lock_guard lock(mutex_); - participants_.clear(); - writers_.clear(); - readers_.clear(); -} - -std::vector RtpsParticipant::discovered_participants() const { - return discovery_.participants(); -} - -std::vector RtpsParticipant::discovered_writers() const { - return discovery_.writers(); -} - -std::vector RtpsParticipant::discovered_readers() const { - return discovery_.readers(); -} - -std::vector RtpsParticipant::writers() const { - std::lock_guard lock(mutex_); - return writers_; -} - -std::vector RtpsParticipant::readers() const { - std::lock_guard lock(mutex_); - return readers_; -} - -RtpsParticipant::PortMapping RtpsParticipant::ports() const { - return compute_port_mapping(config_.domain_id, config_.participant_id); -} - -RtpsParticipant::Guid RtpsParticipant::participant_guid() const { - return {.prefix = guid_prefix_, .entity_id = {.value = kParticipantEntityId}}; -} - -RtpsParticipant::Guid RtpsParticipant::writer_guid(size_t index) const { - return {.prefix = guid_prefix_, - .entity_id = { - .value = entity_id_for_index(static_cast(index), kUserWriterNoKeyKind)}}; -} - -RtpsParticipant::Guid RtpsParticipant::reader_guid(size_t index) const { - return {.prefix = guid_prefix_, - .entity_id = { - .value = entity_id_for_index(static_cast(index), kUserReaderNoKeyKind)}}; -} - -std::vector RtpsParticipant::build_announce_message() const { - return build_spdp_announce_message(); -} - -std::vector RtpsParticipant::build_spdp_announce_message() const { - ByteWriter parameters; - append_parameter_protocol_version(parameters, ProtocolVersion{}); - append_parameter_vendor_id(parameters, VendorId{}); - append_parameter_u32(parameters, ParameterId::PID_DOMAIN_ID, config_.domain_id); - append_parameter_guid(parameters, ParameterId::PID_PARTICIPANT_GUID, participant_guid()); - append_parameter_locator( - parameters, ParameterId::PID_METATRAFFIC_MULTICAST_LOCATOR, - Locator::udp_v4(config_.metatraffic_multicast_group, ports().metatraffic_multicast)); - append_parameter_locator( - parameters, ParameterId::PID_METATRAFFIC_UNICAST_LOCATOR, - Locator::udp_v4(config_.advertised_address, ports().metatraffic_unicast)); - append_parameter_locator(parameters, ParameterId::PID_DEFAULT_UNICAST_LOCATOR, - Locator::udp_v4(config_.advertised_address, ports().user_unicast)); - if (config_.use_multicast_for_user_data) { - append_parameter_locator(parameters, ParameterId::PID_DEFAULT_MULTICAST_LOCATOR, - Locator::udp_v4(config_.user_multicast_group, ports().user_multicast)); - } - append_parameter_duration(parameters, ParameterId::PID_PARTICIPANT_LEASE_DURATION, - kDefaultLeaseDurationSeconds, kDefaultLeaseDurationNanoseconds); - append_parameter_u32(parameters, ParameterId::PID_BUILTIN_ENDPOINT_SET, kBuiltinEndpointSet); - std::string enclave_text = "enclave=" + config_.enclave + ";"; - append_parameter_octet_sequence( - parameters, ParameterId::PID_USER_DATA, - std::span{reinterpret_cast(enclave_text.data()), - enclave_text.size()}); - append_parameter_string_cdr(parameters, ParameterId::PID_ENTITY_NAME, config_.node_name); - append_parameter_sentinel(parameters); - - auto payload = build_parameter_list_payload(parameters); - return build_message(guid_prefix_, {.value = kEntityIdUnknown}, {.value = kSpdpWriterEntityId}, - next_spdp_sequence_number(), payload) - .serialize(); -} - -std::vector -RtpsParticipant::build_sedp_publication_payload(const WriterConfig &writer_config) const { - ByteWriter parameters; - auto guid = writer_guid(writer_config.entity_index); - append_parameter_guid(parameters, ParameterId::PID_ENDPOINT_GUID, guid); - append_parameter_locator(parameters, ParameterId::PID_UNICAST_LOCATOR, - Locator::udp_v4(config_.advertised_address, ports().user_unicast)); - if (!writer_config.multicast_group.empty()) { - append_parameter_locator( - parameters, ParameterId::PID_MULTICAST_LOCATOR, - Locator::udp_v4(writer_config.multicast_group, ports().user_multicast)); - } - append_parameter_guid(parameters, ParameterId::PID_PARTICIPANT_GUID, participant_guid()); - append_parameter_string_cdr(parameters, ParameterId::PID_TOPIC_NAME, writer_config.topic_name); - append_parameter_string_cdr(parameters, ParameterId::PID_TYPE_NAME, writer_config.type_name); - append_parameter_key_hash(parameters, guid); - append_parameter_u32(parameters, ParameterId::PID_TYPE_MAX_SIZE_SERIALIZED, - kUInt32SerializedSize); - append_parameter_protocol_version(parameters, ProtocolVersion{}); - append_parameter_vendor_id(parameters, VendorId{}); - append_parameter_durability(parameters); - append_parameter_liveliness(parameters); - append_parameter_reliability(parameters, writer_config.reliability); - append_parameter_history(parameters, writer_config.history_depth); - append_parameter_sentinel(parameters); - return build_parameter_list_payload(parameters); -} - -std::vector -RtpsParticipant::build_sedp_publication_message(const WriterConfig &writer_config) const { - return build_message(guid_prefix_, {.value = kSedpPublicationsReaderEntityId}, - {.value = kSedpPublicationsWriterEntityId}, - next_sedp_publication_sequence_number(), - build_sedp_publication_payload(writer_config)) - .serialize(); -} - -std::vector -RtpsParticipant::build_sedp_subscription_payload(const ReaderConfig &reader_config) const { - ByteWriter parameters; - auto guid = reader_guid(reader_config.entity_index); - append_parameter_guid(parameters, ParameterId::PID_ENDPOINT_GUID, guid); - append_parameter_locator(parameters, ParameterId::PID_UNICAST_LOCATOR, - Locator::udp_v4(config_.advertised_address, ports().user_unicast)); - if (!reader_config.multicast_group.empty()) { - append_parameter_locator( - parameters, ParameterId::PID_MULTICAST_LOCATOR, - Locator::udp_v4(reader_config.multicast_group, ports().user_multicast)); - } - append_parameter_bool(parameters, ParameterId::PID_EXPECTS_INLINE_QOS, false); - append_parameter_guid(parameters, ParameterId::PID_PARTICIPANT_GUID, participant_guid()); - append_parameter_string_cdr(parameters, ParameterId::PID_TOPIC_NAME, reader_config.topic_name); - append_parameter_string_cdr(parameters, ParameterId::PID_TYPE_NAME, reader_config.type_name); - append_parameter_key_hash(parameters, guid); - append_parameter_protocol_version(parameters, ProtocolVersion{}); - append_parameter_vendor_id(parameters, VendorId{}); - append_parameter_durability(parameters); - append_parameter_liveliness(parameters); - append_parameter_reliability(parameters, reader_config.reliability); - append_parameter_history(parameters); - append_parameter_sentinel(parameters); - return build_parameter_list_payload(parameters); -} - -std::vector -RtpsParticipant::build_sedp_subscription_message(const ReaderConfig &reader_config) const { - return build_message(guid_prefix_, {.value = kSedpSubscriptionsReaderEntityId}, - {.value = kSedpSubscriptionsWriterEntityId}, - next_sedp_subscription_sequence_number(), - build_sedp_subscription_payload(reader_config)) - .serialize(); -} - -std::vector -RtpsParticipant::build_data_message(const WriterConfig &writer_config, - std::span cdr_payload) const { - return build_data_message_with_sequence_number( - writer_config, cdr_payload, next_user_data_sequence_number(writer_config.entity_index), - EntityId{}); -} - -std::vector RtpsParticipant::build_data_message_with_sequence_number( - const WriterConfig &writer_config, std::span cdr_payload, - int64_t sequence_number, EntityId reader_id) const { - // Standard RTPS: the DATA submessage serializedPayload is exactly the CDR-encapsulated sample. - // The topic is identified by the writer GUID (resolved by the receiver via SEDP discovery), so no - // topic name or other framing is embedded in the payload. readerId addresses a specific matched - // reader for unicast sends (ENTITYID_UNKNOWN for multicast, which targets all matched readers). - auto guid = writer_guid(writer_config.entity_index); - return build_message(guid_prefix_, reader_id, guid.entity_id, sequence_number, cdr_payload) - .serialize(); -} - -void RtpsParticipant::store_reliable_sample(const WriterConfig &writer_config, - int64_t sequence_number, - std::span cdr_payload) { - std::lock_guard lock(reliable_mutex_); - auto &state = writer_reliable_states_[writer_config.entity_index]; - state.history[sequence_number].assign(cdr_payload.begin(), cdr_payload.end()); - state.last_sequence_number = std::max(state.last_sequence_number, sequence_number); - // Bound the cache to the configured KEEP_LAST depth, dropping the oldest samples. - uint32_t depth = std::max(writer_config.history_depth, 1); - while (state.history.size() > depth) { - state.history.erase(state.history.begin()); - } -} - -bool RtpsParticipant::publish(std::string_view topic_name, std::span cdr_payload) { - WriterConfig writer_config; - { - std::lock_guard lock(mutex_); - auto iterator = - std::find_if(writers_.begin(), writers_.end(), - [topic_name](const auto &writer) { return writer.topic_name == topic_name; }); - if (iterator == writers_.end()) { - logger_.warn("No writer registered for topic '{}'", topic_name); - return false; - } - writer_config = *iterator; - } - - if (!user_unicast_receiver_) { - return false; - } - - // Allocate the sequence number once so a reliable writer can cache the sample under the same - // sequence number it sends on the wire. - int64_t sequence_number = next_user_data_sequence_number(writer_config.entity_index); - const bool reliable = writer_config.reliability == ReliabilityKind::RELIABLE; - if (reliable) { - store_reliable_sample(writer_config, sequence_number, cdr_payload); - } - - auto destinations = build_user_send_configs(topic_name, writer_config); - if (destinations.empty()) { - logger_.warn("No send destinations available for topic '{}'", topic_name); - return false; - } - - // Build the DATA per destination so each unicast send addresses its target reader's entity id - // (multicast destinations use ENTITYID_UNKNOWN). The sequence number and payload are identical. - bool sent = false; - for (const auto &destination : destinations) { - auto payload = build_data_message_with_sequence_number(writer_config, cdr_payload, - sequence_number, destination.reader_id); - sent = user_unicast_receiver_->send(payload, destination.send_config) || sent; - } - - // Reliable writers announce the available sequence-number range so matched reliable readers can - // detect gaps and request retransmission (Phase 3). The retransmission response to ACKNACK is not - // wired up yet, but the HEARTBEAT exchange is interoperable with DDS/ROS 2 peers. - if (reliable) { - send_heartbeat_for_writer(writer_config); - } - return sent; -} - -bool RtpsParticipant::send_heartbeat_for_writer(const WriterConfig &writer_config) { - if (!user_unicast_receiver_) { - return false; - } - - // Snapshot the available sequence-number range and bump the heartbeat count under - // reliable_mutex_. An empty history is advertised as firstSN=1, lastSN=0 (no samples available). - int64_t first_sn = 1; - int64_t last_sn = 0; - uint32_t count = 0; - { - std::lock_guard lock(reliable_mutex_); - auto &state = writer_reliable_states_[writer_config.entity_index]; - if (!state.history.empty()) { - first_sn = state.history.begin()->first; - last_sn = state.last_sequence_number; - } - count = ++state.heartbeat_count; - } - - auto writer_entity_id = writer_guid(writer_config.entity_index).entity_id; - - // Snapshot the matched reliable readers for this topic. - std::vector matched_readers; - for (auto &reader : discovery_.readers()) { - if (reader.is_reader && reader.topic_name == writer_config.topic_name && - reader.reliability == ReliabilityKind::RELIABLE) { - matched_readers.push_back(std::move(reader)); - } - } - - bool sent = false; - for (const auto &reader : matched_readers) { - if (!has_valid_locator(reader.unicast_locator)) { - continue; - } - Message message; - message.header.guid_prefix = guid_prefix_; - // INFO_DST tells the destination participant which entity the HEARTBEAT is for, so DDS/ROS 2 - // peers route it to the right reader. - message.submessages.push_back(build_info_dst_submessage(reader.guid.prefix)); - message.submessages.push_back(build_heartbeat_submessage( - reader.guid.entity_id, writer_entity_id, first_sn, last_sn, count, /*final=*/false)); - auto bytes = message.serialize(); - UdpSocket::SendConfig send_config{ - .ip_address = reader.unicast_locator.address_string(), - .port = static_cast(reader.unicast_locator.port), - .is_multicast_endpoint = false, - }; - sent = user_unicast_receiver_->send(bytes, send_config) || sent; - } - return sent; -} - -bool RtpsParticipant::send_heartbeats_now() { - std::vector reliable_writers; - { - std::lock_guard lock(mutex_); - std::copy_if( - writers_.begin(), writers_.end(), std::back_inserter(reliable_writers), - [](const WriterConfig &writer) { return writer.reliability == ReliabilityKind::RELIABLE; }); - } - bool sent = false; - for (const auto &writer : reliable_writers) { - sent = send_heartbeat_for_writer(writer) || sent; - } - return sent; -} - -void RtpsParticipant::drain_reader_frontier(ReaderReliableState &state, - std::vector> &delivered) { - while (true) { - const int64_t next = state.highest_delivered + 1; - auto buffered = state.reorder.find(next); - if (buffered != state.reorder.end()) { - delivered.push_back(std::move(buffered->second)); - state.reorder.erase(buffered); - state.highest_delivered = next; - continue; - } - auto skipped = state.irrelevant.find(next); - if (skipped != state.irrelevant.end()) { - state.irrelevant.erase(skipped); // irrelevant sample: advance past it without delivering - state.highest_delivered = next; - continue; - } - break; - } -} - -void RtpsParticipant::apply_gap(ReaderReliableState &state, int64_t gap_start, - int64_t gap_list_base, - const std::vector &bitmap_irrelevant, - std::vector> &delivered) { - constexpr int64_t kMaxGapRange = 4096; - // Contiguous irrelevant range [gap_start, gap_list_base - 1]. - const int64_t range_end = gap_list_base - 1; - if (range_end > state.highest_delivered) { - if (gap_start <= state.highest_delivered + 1) { - // Touches the frontier: advance directly past the whole irrelevant range. - state.highest_delivered = range_end; - } else if (range_end - gap_start <= kMaxGapRange) { - // A hole precedes the gap: remember the irrelevant SNs so the frontier skips them later. - for (int64_t sn = gap_start; sn <= range_end; sn++) { - if (sn > state.highest_delivered) { - state.irrelevant.insert(sn); - } - } - } else { - // Unreasonably large gap ahead of our frontier: we are hopelessly behind; jump past it. - state.highest_delivered = range_end; - } - } - // Individual irrelevant sequence numbers (the set bits) at/after gap_list_base. - for (int64_t sn : bitmap_irrelevant) { - if (sn > state.highest_delivered) { - state.irrelevant.insert(sn); - } - } - // Drop anything that is now at/below the frontier. - while (!state.reorder.empty() && state.reorder.begin()->first <= state.highest_delivered) { - state.reorder.erase(state.reorder.begin()); - } - while (!state.irrelevant.empty() && *state.irrelevant.begin() <= state.highest_delivered) { - state.irrelevant.erase(state.irrelevant.begin()); - } - drain_reader_frontier(state, delivered); -} - -void RtpsParticipant::deliver_reliable_sample( - uint32_t reader_entity_index, const Guid &writer_guid, int64_t sequence_number, - std::span payload, - const std::function)> &on_sample) { - // Collect the in-order samples to deliver under the lock, then invoke the callback after - // releasing it (the callback may re-enter the participant, e.g. publish a response). - std::vector> to_deliver; - { - std::lock_guard lock(reliable_mutex_); - auto key = fmt::format("{}#{}", reader_entity_index, writer_guid.to_string()); - auto &state = reader_reliable_states_[key]; - if (sequence_number <= state.highest_delivered) { - // Duplicate (e.g. a retransmission of an already-delivered sample); drop it. - return; - } - if (sequence_number == state.highest_delivered + 1) { - // Next expected sample: deliver it, then drain any now-contiguous buffered / irrelevant SNs. - to_deliver.emplace_back(payload.begin(), payload.end()); - state.highest_delivered = sequence_number; - drain_reader_frontier(state, to_deliver); - } else if (state.reorder.find(sequence_number) == state.reorder.end() && - state.reorder.size() < config_.reliable_reorder_depth) { - // Out of order: buffer for in-order delivery once the gap is filled. If the buffer is full - // the sample is dropped and will be re-requested via the next ACKNACK. - state.reorder[sequence_number].assign(payload.begin(), payload.end()); - } - } - for (const auto &sample : to_deliver) { - on_sample(sample); - } -} - -void RtpsParticipant::send_acknack_for_heartbeat(const GuidPrefix &writer_prefix, - const EntityId &writer_id, int64_t first_sn, - int64_t last_sn, uint32_t heartbeat_count, - bool heartbeat_final) { - if (!user_unicast_receiver_) { - return; - } - Guid remote_writer_guid{.prefix = writer_prefix, .entity_id = writer_id}; - - // Resolve the writer's topic + where to send the ACKNACK from discovery, then the matched - // reliable local readers. - auto writer = discovery_.find_writer(remote_writer_guid); - if (!writer || writer->reliability != ReliabilityKind::RELIABLE) { - return; - } - const Locator writer_locator = writer->unicast_locator; - std::vector matched_reader_indices; - { - std::lock_guard lock(mutex_); - for (const auto &reader_config : readers_) { - if (reader_config.topic_name == writer->topic_name && - reader_config.reliability == ReliabilityKind::RELIABLE) { - matched_reader_indices.push_back(reader_config.entity_index); - } - } - } - if (matched_reader_indices.empty()) { - return; - } - std::string participant_address; - uint16_t participant_user_unicast = 0; - if (auto participant = discovery_.find_participant_by_prefix(writer_prefix)) { - participant_address = participant->address; - participant_user_unicast = participant->ports.user_unicast; - } - - // Prefer the writer's advertised unicast locator; fall back to the participant's user-unicast - // endpoint (the raw datagram source port is the writer's ephemeral send port, not its receiver). - std::string dest_address; - uint16_t dest_port = 0; - if (has_valid_locator(writer_locator)) { - dest_address = writer_locator.address_string(); - dest_port = static_cast(writer_locator.port); - } else if (!participant_address.empty() && participant_user_unicast != 0) { - dest_address = participant_address; - dest_port = participant_user_unicast; - } else { - return; - } - - for (uint32_t reader_entity_index : matched_reader_indices) { - auto reader_entity_id = reader_guid(reader_entity_index).entity_id; - SequenceNumberSet reader_sn_state; - uint32_t count = 0; - { - std::lock_guard lock(reliable_mutex_); - auto key = fmt::format("{}#{}", reader_entity_index, remote_writer_guid.to_string()); - auto &state = reader_reliable_states_[key]; - // Samples below firstSN are no longer available from the writer (history purged); treat the - // gap as permanently lost and skip past it so we do not NACK samples that no longer exist. - if (first_sn > state.highest_delivered + 1) { - logger_.warn("Reliable reader missed samples [{}, {}] from writer {} (writer history " - "advanced past them)", - state.highest_delivered + 1, first_sn - 1, remote_writer_guid.to_string()); - state.highest_delivered = first_sn - 1; - while (!state.reorder.empty() && state.reorder.begin()->first <= state.highest_delivered) { - state.reorder.erase(state.reorder.begin()); - } - } - // Ignore a stale heartbeat that does not require a response. - if (heartbeat_count <= state.last_heartbeat_count && heartbeat_final) { - continue; - } - state.last_heartbeat_count = std::max(state.last_heartbeat_count, heartbeat_count); - - int64_t base = state.highest_delivered + 1; - reader_sn_state.base = base; - bool any_missing = false; - for (int64_t sn = base; - sn <= last_sn && (sn - base) < static_cast(SequenceNumberSet::kMaxBits); sn++) { - if (state.reorder.find(sn) == state.reorder.end()) { - reader_sn_state.set(sn); - any_missing = true; - } - } - if (!any_missing) { - // Positive acknowledgement of everything up to lastSN (empty set, base = next expected). - reader_sn_state = SequenceNumberSet{}; - reader_sn_state.base = (last_sn >= base) ? last_sn + 1 : base; - } - count = ++state.acknack_count; - } - - Message message; - message.header.guid_prefix = guid_prefix_; - message.submessages.push_back(build_info_dst_submessage(writer_prefix)); - message.submessages.push_back(build_acknack_submessage(reader_entity_id, writer_id, - reader_sn_state, count, /*final=*/true)); - auto bytes = message.serialize(); - UdpSocket::SendConfig send_config{ - .ip_address = dest_address, - .port = dest_port, - .is_multicast_endpoint = false, - }; - user_unicast_receiver_->send(bytes, send_config); - } -} - -int64_t RtpsParticipant::next_spdp_sequence_number() const { - return spdp_sequence_number_.fetch_add(1, std::memory_order_relaxed); -} - -int64_t RtpsParticipant::next_sedp_publication_sequence_number() const { - return sedp_publications_sequence_number_.fetch_add(1, std::memory_order_relaxed); -} - -int64_t RtpsParticipant::next_sedp_subscription_sequence_number() const { - return sedp_subscriptions_sequence_number_.fetch_add(1, std::memory_order_relaxed); -} - -int64_t RtpsParticipant::next_user_data_sequence_number(uint32_t entity_index) const { - std::lock_guard lock(sequence_mutex_); - auto iterator = user_data_sequence_numbers_.try_emplace(entity_index, 1).first; - auto &sequence_number = iterator->second; - int64_t current = sequence_number; - sequence_number++; - return current; -} - -RtpsParticipant::PortMapping RtpsParticipant::compute_port_mapping(uint16_t domain_id, - uint16_t participant_id) { - auto base = static_cast(kPortBase) + static_cast(kDomainGain) * domain_id; - auto participant_offset = static_cast(kParticipantGain) * participant_id; - return {.metatraffic_multicast = static_cast(base + kMetatrafficMulticastOffset), - .metatraffic_unicast = - static_cast(base + kMetatrafficUnicastOffset + participant_offset), - .user_multicast = static_cast(base + kUserMulticastOffset), - .user_unicast = static_cast(base + kUserUnicastOffset + participant_offset)}; -} - -void RtpsParticipant::record_builtin_sample(const Guid &writer_guid, int64_t sequence_number) { - std::lock_guard lock(reliable_mutex_); - auto &state = builtin_reader_states_[writer_guid.to_string()]; - if (sequence_number <= state.highest_delivered) { - return; // already accounted for - } - if (sequence_number == state.highest_delivered + 1) { - state.highest_delivered = sequence_number; - while (true) { - auto iterator = state.reorder.find(state.highest_delivered + 1); - if (iterator == state.reorder.end()) { - break; - } - state.reorder.erase(iterator); - state.highest_delivered++; - } - } else { - state.reorder[sequence_number]; // mark this out-of-order SEDP sample as received (key only) - } -} - -void RtpsParticipant::send_builtin_acknack(const GuidPrefix &writer_prefix, - const EntityId &writer_id, int64_t first_sn, - int64_t last_sn, uint32_t heartbeat_count, - bool /*heartbeat_final*/) { - if (!metatraffic_unicast_receiver_) { - return; - } - // Only the reliable builtin SEDP writers need an ACKNACK; map each to our matching builtin - // reader. - EntityId reader_id; - if (writer_id.value == kSedpPublicationsWriterEntityId) { - reader_id.value = kSedpPublicationsReaderEntityId; - } else if (writer_id.value == kSedpSubscriptionsWriterEntityId) { - reader_id.value = kSedpSubscriptionsReaderEntityId; - } else { - return; // SPDP is best-effort; other builtin writers are not tracked. - } - - // Resolve where the peer's builtin SEDP reader receives traffic: its metatraffic unicast - // endpoint. - std::string dest_address; - uint16_t dest_port = 0; - if (auto participant = discovery_.find_participant_by_prefix(writer_prefix)) { - if (has_valid_locator(participant->metatraffic_unicast_locator)) { - dest_address = participant->metatraffic_unicast_locator.address_string(); - dest_port = static_cast(participant->metatraffic_unicast_locator.port); - } else { - dest_address = participant->address; - dest_port = participant->ports.metatraffic_unicast; - } - } - if (dest_address.empty() || dest_port == 0) { - return; - } - - Guid writer_guid{.prefix = writer_prefix, .entity_id = writer_id}; - SequenceNumberSet reader_sn_state; - uint32_t count = 0; - bool any_missing = false; - { - std::lock_guard lock(reliable_mutex_); - auto &state = builtin_reader_states_[writer_guid.to_string()]; - if (first_sn > state.highest_delivered + 1) { - // Samples below firstSN are no longer available from the writer; skip past them. - state.highest_delivered = first_sn - 1; - while (!state.reorder.empty() && state.reorder.begin()->first <= state.highest_delivered) { - state.reorder.erase(state.reorder.begin()); - } - } - state.last_heartbeat_count = std::max(state.last_heartbeat_count, heartbeat_count); - int64_t base = state.highest_delivered + 1; - reader_sn_state.base = base; - for (int64_t sn = base; - sn <= last_sn && (sn - base) < static_cast(SequenceNumberSet::kMaxBits); sn++) { - if (state.reorder.find(sn) == state.reorder.end()) { - reader_sn_state.set(sn); - any_missing = true; - } - } - if (!any_missing) { - // Positive ack of everything announced (empty set, base = next expected). - reader_sn_state = SequenceNumberSet{}; - reader_sn_state.base = (last_sn >= base) ? last_sn + 1 : base; - } - count = ++state.acknack_count; - } - - Message ack_message; - ack_message.header.guid_prefix = guid_prefix_; - ack_message.submessages.push_back(build_info_dst_submessage(writer_prefix)); - // Leave the Final flag unset while samples are still missing so the writer responds promptly. - ack_message.submessages.push_back(build_acknack_submessage(reader_id, writer_id, reader_sn_state, - count, /*final=*/!any_missing)); - auto bytes = ack_message.serialize(); - UdpSocket::SendConfig send_config{.ip_address = dest_address, .port = dest_port}; - metatraffic_unicast_receiver_->send(bytes, send_config); - logger_.debug("Sent builtin ACKNACK to {}:{} for writer {} (base={}, missing={}, count={})", - dest_address, dest_port, writer_guid.to_string(), reader_sn_state.base, any_missing, - count); -} - -bool RtpsParticipant::handle_metatraffic_message(std::vector &data, - const Socket::Info &sender) { - auto message = Message::parse(data); - if (!message) { - return false; - } - - for (const auto &submessage : message->submessages) { - // Reliable peers (e.g. Fast DDS) heartbeat their builtin SEDP writers and will not (re)send - // discovery data until they get an ACKNACK. Reply so we receive their endpoint announcements. - if (submessage.kind == SubmessageKind::HEARTBEAT) { - if (message->header.guid_prefix != guid_prefix_) { - auto heartbeat = parse_heartbeat_submessage(submessage); - if (heartbeat.valid) { - send_builtin_acknack(message->header.guid_prefix, heartbeat.writer_id, heartbeat.first_sn, - heartbeat.last_sn, heartbeat.count, heartbeat.final); - } - } - continue; - } - // A reliable peer ACKNACKs our builtin SEDP writers; resend any NACKed discovery samples. - if (submessage.kind == SubmessageKind::ACKNACK) { - if (message->header.guid_prefix != guid_prefix_) { - auto acknack = parse_acknack_submessage(submessage); - if (acknack.valid) { - retransmit_sedp(message->header.guid_prefix, acknack.reader_id, acknack.writer_id, - requested_sequence_numbers(acknack.reader_sn_state)); - } - } - continue; - } - // A peer GAPs its builtin SEDP writer (samples it will never send); advance past them. - if (submessage.kind == SubmessageKind::GAP) { - if (message->header.guid_prefix != guid_prefix_) { - auto gap = parse_gap_submessage(submessage); - if (gap.valid) { - handle_builtin_gap(message->header.guid_prefix, gap.writer_id, gap.gap_start, - gap.gap_list.base, requested_sequence_numbers(gap.gap_list)); - } - } - continue; - } - - bool valid_data = false; - auto data_view = parse_data_submessage(submessage, valid_data); - if (!valid_data) { - continue; - } - - auto parameters = parse_parameter_list(data_view.serialized_payload); - if (parameters.empty()) { - continue; - } - - if (data_view.writer_id.value == kSpdpWriterEntityId) { - auto maybe_participant_guid_parameter = - find_parameter(parameters, ParameterId::PID_PARTICIPANT_GUID); - if (!maybe_participant_guid_parameter) { - continue; - } - auto maybe_participant_guid = parse_guid(maybe_participant_guid_parameter->value); - if (!maybe_participant_guid || is_same_guid_prefix(*maybe_participant_guid, guid_prefix_)) { - continue; - } - - // Parse only the fields actually present in this announcement; the upsert below merges them - // into the existing record, so a later (trimmed) announcement does not erase what we learned. - std::optional name; - if (auto parameter = find_parameter(parameters, ParameterId::PID_ENTITY_NAME)) { - name = parse_cdr_string(parameter->value); - } - std::optional enclave; - if (auto parameter = find_parameter(parameters, ParameterId::PID_USER_DATA)) { - if (auto user_data = parse_octet_sequence(parameter->value)) { - enclave = extract_enclave(*user_data); - } - } - std::optional builtin_endpoints; - if (auto parameter = find_parameter(parameters, ParameterId::PID_BUILTIN_ENDPOINT_SET)) { - builtin_endpoints = parse_u32_le(parameter->value); - } - std::optional meta_unicast; - if (auto parameter = - find_parameter(parameters, ParameterId::PID_METATRAFFIC_UNICAST_LOCATOR)) { - meta_unicast = parse_locator(parameter->value); - } - std::optional meta_multicast; - if (auto parameter = - find_parameter(parameters, ParameterId::PID_METATRAFFIC_MULTICAST_LOCATOR)) { - meta_multicast = parse_locator(parameter->value); - } - std::optional default_unicast; - if (auto parameter = find_parameter(parameters, ParameterId::PID_DEFAULT_UNICAST_LOCATOR)) { - default_unicast = parse_locator(parameter->value); - } - std::optional default_multicast; - if (auto parameter = find_parameter(parameters, ParameterId::PID_DEFAULT_MULTICAST_LOCATOR)) { - default_multicast = parse_locator(parameter->value); - } - const std::string sender_address = sender.address; - - auto result = discovery_.upsert_participant( - *maybe_participant_guid, [&](ParticipantProxy &participant) { - participant.participant_guid = *maybe_participant_guid; - participant.guid_prefix = maybe_participant_guid->prefix; - if (participant.address.empty()) { - participant.address = sender_address; - } - if (name) { - participant.name = *name; - } - if (enclave) { - participant.enclave = *enclave; - } - if (builtin_endpoints) { - participant.builtin_endpoints = *builtin_endpoints; - } - if (meta_unicast) { - participant.ports.metatraffic_unicast = static_cast(meta_unicast->port); - if (has_valid_locator(*meta_unicast)) { - participant.metatraffic_unicast_locator = *meta_unicast; - } - } - if (meta_multicast) { - participant.ports.metatraffic_multicast = static_cast(meta_multicast->port); - if (has_valid_locator(*meta_multicast)) { - participant.metatraffic_multicast_locator = *meta_multicast; - } - } - if (default_unicast) { - participant.ports.user_unicast = static_cast(default_unicast->port); - if (has_valid_locator(*default_unicast)) { - participant.default_unicast_locator = *default_unicast; - participant.address = default_unicast->address_string(); - } - } - if (default_multicast) { - participant.ports.user_multicast = static_cast(default_multicast->port); - if (has_valid_locator(*default_multicast)) { - participant.default_multicast_locator = *default_multicast; - } - } - }); - - logger_.debug("SPDP parsed participant {} from src {}: meta_uc={} meta_mc={} default_uc={} " - "default_mc={} -> stored(address={}, ports.meta_uc={}, ports.user_uc={})", - maybe_participant_guid->prefix.to_string(), sender_address, - locator_to_string(meta_unicast), locator_to_string(meta_multicast), - locator_to_string(default_unicast), locator_to_string(default_multicast), - result.value.address, result.value.ports.metatraffic_unicast, - result.value.ports.user_unicast); - - if (result.is_new) { - const auto &participant = result.value; - logger_.info("SPDP discovered participant '{}' at {} (meta {}, user {})", - participant.name.empty() ? participant.guid_prefix.to_string() - : participant.name, - participant.address, participant.ports.metatraffic_unicast, - participant.ports.user_unicast); - send_sedp_announcements_to(participant); - if (config_.on_participant_discovered) { - config_.on_participant_discovered(participant); - } - } - continue; - } - - bool is_reader = false; - if (data_view.writer_id.value == kSedpPublicationsWriterEntityId) { - is_reader = false; - } else if (data_view.writer_id.value == kSedpSubscriptionsWriterEntityId) { - is_reader = true; - } else { - continue; - } - - // Record the SEDP sample so we can correctly ACKNACK the peer's builtin SEDP heartbeats. - record_builtin_sample({.prefix = message->header.guid_prefix, .entity_id = data_view.writer_id}, - data_view.writer_sn); - - auto maybe_endpoint_guid_parameter = find_parameter(parameters, ParameterId::PID_ENDPOINT_GUID); - if (!maybe_endpoint_guid_parameter) { - continue; - } - auto maybe_endpoint_guid = parse_guid(maybe_endpoint_guid_parameter->value); - if (!maybe_endpoint_guid || is_same_guid_prefix(*maybe_endpoint_guid, guid_prefix_)) { - continue; - } - - // Parse only the present fields; the upsert below merges them into the existing endpoint. - const Guid endpoint_guid = *maybe_endpoint_guid; - std::optional endpoint_participant_guid; - if (auto parameter = find_parameter(parameters, ParameterId::PID_PARTICIPANT_GUID)) { - endpoint_participant_guid = parse_guid(parameter->value); - } - std::optional topic_name; - if (auto parameter = find_parameter(parameters, ParameterId::PID_TOPIC_NAME)) { - topic_name = parse_cdr_string(parameter->value); - } - std::optional type_name; - if (auto parameter = find_parameter(parameters, ParameterId::PID_TYPE_NAME)) { - type_name = parse_cdr_string(parameter->value); - } - std::optional unicast_locator; - if (auto parameter = find_parameter(parameters, ParameterId::PID_UNICAST_LOCATOR)) { - unicast_locator = parse_locator(parameter->value); - } - std::vector multicast_locators; - for (const auto &locator_parameter : - find_parameters(parameters, ParameterId::PID_MULTICAST_LOCATOR)) { - if (auto parsed = parse_locator(locator_parameter.value)) { - multicast_locators.push_back(*parsed); - } - } - std::optional reliability; - if (auto parameter = find_parameter(parameters, ParameterId::PID_RELIABILITY)) { - reliability = parse_reliability(parameter->value); - } - std::optional expects_inline_qos; - if (auto parameter = find_parameter(parameters, ParameterId::PID_EXPECTS_INLINE_QOS)) { - expects_inline_qos = parse_bool(parameter->value); - } - - auto result = - discovery_.upsert_endpoint(is_reader, endpoint_guid, [&](EndpointProxy &endpoint) { - endpoint.guid = endpoint_guid; - endpoint.is_reader = is_reader; - if (endpoint_participant_guid) { - endpoint.participant_guid = *endpoint_participant_guid; - } - if (endpoint.participant_guid.entity_id.value == std::array{}) { - // Fall back to the implicit participant entity within the endpoint's prefix. - endpoint.participant_guid = {.prefix = endpoint_guid.prefix, - .entity_id = {.value = kParticipantEntityId}}; - } - if (topic_name) { - endpoint.topic_name = *topic_name; - } - if (type_name) { - endpoint.type_name = *type_name; - } - if (unicast_locator && has_valid_locator(*unicast_locator)) { - endpoint.unicast_locator = *unicast_locator; - } - if (!multicast_locators.empty()) { - endpoint.multicast_locators = multicast_locators; - } - if (reliability) { - endpoint.reliability = *reliability; - } - if (expects_inline_qos) { - endpoint.expects_inline_qos = *expects_inline_qos; - } - }); - - logger_.debug( - "SEDP parsed {} {} topic '{}' [{}] reliability={} unicast={} multicast_count={} -> " - "participant {}", - is_reader ? "reader" : "writer", endpoint_guid.to_string(), result.value.topic_name, - result.value.type_name, - result.value.reliability == ReliabilityKind::RELIABLE ? "RELIABLE" : "BEST_EFFORT", - locator_to_string(result.value.unicast_locator), result.value.multicast_locators.size(), - result.value.participant_guid.to_string()); - - if (result.is_new) { - const auto &endpoint = result.value; - logger_.info("SEDP discovered {} '{}' [{}] from participant {}", - endpoint.is_reader ? "reader" : "writer", endpoint.topic_name, - endpoint.type_name, endpoint.participant_guid.to_string()); - if (config_.on_endpoint_discovered) { - config_.on_endpoint_discovered(endpoint); - } - } - } - return false; -} - -bool RtpsParticipant::handle_user_message(std::vector &data, const Socket::Info &sender) { - auto message = Message::parse(data); - if (!message) { - return false; - } - if (message->header.guid_prefix == guid_prefix_) { - return false; - } - - for (const auto &submessage : message->submessages) { - // Reliable-QoS submessages (Phase 0: parsed and logged; the writer/reader - // state machines that act on them land in later phases — see - // RELIABLE_RTPS_PLAN.md). - if (submessage.kind == SubmessageKind::HEARTBEAT) { - auto heartbeat = parse_heartbeat_submessage(submessage); - if (heartbeat.valid) { - logger_.debug("HEARTBEAT from {} (writer {}, reader {}): firstSN={} lastSN={} count={} " - "final={}", - message->header.guid_prefix.to_string(), heartbeat.writer_id.to_string(), - heartbeat.reader_id.to_string(), heartbeat.first_sn, heartbeat.last_sn, - heartbeat.count, heartbeat.final); - // A matched reliable reader replies with an ACKNACK of the missing sequence numbers in - // [firstSN, lastSN] so the writer can retransmit them. - send_acknack_for_heartbeat(message->header.guid_prefix, heartbeat.writer_id, - heartbeat.first_sn, heartbeat.last_sn, heartbeat.count, - heartbeat.final); - } - continue; - } - if (submessage.kind == SubmessageKind::ACKNACK) { - auto acknack = parse_acknack_submessage(submessage); - if (acknack.valid) { - logger_.debug( - "ACKNACK from {} (writer {}, reader {}): base={} numBits={} count={} final={}", - message->header.guid_prefix.to_string(), acknack.writer_id.to_string(), - acknack.reader_id.to_string(), acknack.reader_sn_state.base, - acknack.reader_sn_state.num_bits, acknack.count, acknack.final); - // A matched reliable writer resends the NACKed sequence numbers from its history. - retransmit_user_data(message->header.guid_prefix, acknack.reader_id, acknack.writer_id, - requested_sequence_numbers(acknack.reader_sn_state)); - } - continue; - } - // A reliable writer GAPs samples it will never send (evicted/irrelevant); advance past them so - // we stop NACKing them and release any buffered samples blocked behind them. - if (submessage.kind == SubmessageKind::GAP) { - auto gap = parse_gap_submessage(submessage); - if (gap.valid) { - handle_user_gap(message->header.guid_prefix, gap.writer_id, gap.gap_start, - gap.gap_list.base, requested_sequence_numbers(gap.gap_list)); - } - continue; - } - - bool valid_data = false; - auto data_view = parse_data_submessage(submessage, valid_data); - if (!valid_data) { - continue; - } - - // Standard RTPS: the sample's topic is identified by the writer GUID, which we resolve through - // SEDP discovery state. Build the remote writer GUID from the message prefix + DATA writerId, - // then look up its topic and reliability among discovered writers, and collect the matching - // local reader callbacks under a single lock. - Guid remote_writer_guid{.prefix = message->header.guid_prefix, - .entity_id = data_view.writer_id}; - struct MatchedReader { - uint32_t entity_index{0}; - bool reliable{false}; - std::function)> on_sample{}; - }; - auto writer = discovery_.find_writer(remote_writer_guid); - if (!writer) { - // Sample arrived before the writer was discovered via SEDP; drop it (best-effort). - logger_.debug("Received DATA for unknown writer {} from {} (not discovered via SEDP yet)", - remote_writer_guid.to_string(), sender.address); - continue; - } - const bool writer_is_reliable = writer->reliability == ReliabilityKind::RELIABLE; - std::vector matched_readers; - { - std::lock_guard lock(mutex_); - for (const auto &reader_config : readers_) { - if (reader_config.topic_name == writer->topic_name && reader_config.on_sample) { - matched_readers.push_back( - {.entity_index = reader_config.entity_index, - .reliable = reader_config.reliability == ReliabilityKind::RELIABLE, - .on_sample = reader_config.on_sample}); - } - } - } - - if (matched_readers.empty()) { - continue; - } - // The reliable handshake (dedup + in-order delivery) runs only between endpoints that both - // advertise RELIABLE; otherwise the sample is delivered best-effort (immediately, as received). - for (const auto &reader : matched_readers) { - if (writer_is_reliable && reader.reliable) { - deliver_reliable_sample(reader.entity_index, remote_writer_guid, data_view.writer_sn, - data_view.serialized_payload, reader.on_sample); - } else { - reader.on_sample(data_view.serialized_payload); - } - } - } - return false; -} - -bool RtpsParticipant::ensure_user_multicast_receivers_started(const std::string &extra_group) { - if (!started_.load()) { - return true; - } - - std::vector desired_groups; - auto add_group = [&desired_groups](const std::string &group) { - if (!group.empty() && - std::find(desired_groups.begin(), desired_groups.end(), group) == desired_groups.end()) { - desired_groups.push_back(group); - } - }; - if (config_.use_multicast_for_user_data) { - add_group(config_.user_multicast_group); - } - // Include the group of a reader being added before it is persisted in readers_, so the receiver - // can be brought up (and any failure surfaced) without leaving the reader half-registered. - add_group(extra_group); - { - std::lock_guard lock(mutex_); - for (const auto &reader_config : readers_) { - add_group(reader_config.multicast_group); - } - } - - auto port_mapping = ports(); - // receivers_mutex_ (not mutex_) guards user_multicast_receivers_; desired_groups was built above - // under mutex_, which has already been released, so the two locks are never held nested. - std::lock_guard receivers_lock(receivers_mutex_); - for (const auto &group : desired_groups) { - auto existing = - std::find_if(user_multicast_receivers_.begin(), user_multicast_receivers_.end(), - [&group](const auto &receiver) { return receiver.multicast_group == group; }); - if (existing != user_multicast_receivers_.end()) { - continue; - } - - auto socket = - std::make_unique(UdpSocket::Config{.log_level = config_.socket_log_level}); - auto task_config = config_.receive_task_config; - task_config.name = fmt::format("{}_user_mc_{}", config_.receive_task_config.name, - user_multicast_receivers_.size()); - auto receive_config = UdpSocket::ReceiveConfig{ - .port = port_mapping.user_multicast, - .buffer_size = 4096, - .is_multicast_endpoint = true, - .multicast_group = group, - .multicast_interface = config_.bind_address, - .on_receive_callback = [this](auto &data, - const auto &sender) -> std::optional> { - handle_user_message(data, sender); - return std::nullopt; - }, - }; - if (!socket->start_receiving(task_config, receive_config)) { - logger_.error("Failed to start user multicast receiver for group {}", group); - return false; - } - user_multicast_receivers_.push_back({ - .multicast_group = group, - .socket = std::move(socket), - }); - } - return true; -} - -std::vector -RtpsParticipant::build_user_send_configs(std::string_view topic_name, - const WriterConfig &writer_config) const { - std::vector destinations; - const std::string &multicast_interface = config_.bind_address; - auto add_destination = [&destinations, &multicast_interface](std::string ip_address, - uint16_t port, bool is_multicast, - const EntityId &reader_id) { - if (ip_address.empty() || port == 0) { - return; - } - auto existing = std::find_if( - destinations.begin(), destinations.end(), [&](const UserDataDestination &destination) { - return destination.send_config.ip_address == ip_address && - destination.send_config.port == port && - destination.send_config.is_multicast_endpoint == is_multicast && - destination.reader_id == reader_id; - }); - if (existing == destinations.end()) { - destinations.push_back( - {.send_config = {.ip_address = std::move(ip_address), - .port = port, - .is_multicast_endpoint = is_multicast, - .multicast_interface = - is_multicast ? multicast_interface : std::string{}}, - .reader_id = reader_id}); - } - }; - - // Multicast sends target all matched readers, so the DATA readerId is left as ENTITYID_UNKNOWN. - if (!writer_config.multicast_group.empty()) { - add_destination(writer_config.multicast_group, ports().user_multicast, true, EntityId{}); - return destinations; - } - - if (config_.use_multicast_for_user_data) { - add_destination(config_.user_multicast_group, ports().user_multicast, true, EntityId{}); - return destinations; - } - - for (const auto &reader : discovery_.readers()) { - if (!reader.is_reader || reader.topic_name != topic_name) { - continue; - } - - bool used_multicast = false; - for (const auto &locator : reader.multicast_locators) { - if (!has_valid_locator(locator)) { - continue; - } - // Multicast to a reader's group: addressed to all readers in the group (readerId UNKNOWN). - add_destination(locator.address_string(), static_cast(locator.port), true, - EntityId{}); - used_multicast = true; - } - if (used_multicast) { - continue; - } - - // Unicast to a specific reader: address the DATA to that reader's entity id. - if (has_valid_locator(reader.unicast_locator)) { - add_destination(reader.unicast_locator.address_string(), - static_cast(reader.unicast_locator.port), false, - reader.guid.entity_id); - continue; - } - - if (auto participant = discovery_.find_participant_by_prefix(reader.participant_guid.prefix)) { - add_destination(participant->address, participant->ports.user_unicast, false, - reader.guid.entity_id); - } - } - - return destinations; -} - -bool RtpsParticipant::send_spdp_announce_now() { - if (!metatraffic_unicast_receiver_) { - return false; - } - auto payload = build_spdp_announce_message(); - auto send_config = UdpSocket::SendConfig{ - .ip_address = config_.metatraffic_multicast_group, - .port = ports().metatraffic_multicast, - .is_multicast_endpoint = true, - .multicast_interface = config_.bind_address, - }; - return metatraffic_unicast_receiver_->send(payload, send_config); -} - -bool RtpsParticipant::send_sedp_announcements_to(const ParticipantProxy &participant) { - // Prefer the peer's advertised metatraffic unicast locator (its full address + port); fall back - // to participant.address (the SPDP source / default-unicast address) + the derived metatraffic - // port. - std::string meta_address; - uint16_t meta_port = 0; - const char *address_source = "metatraffic_unicast_locator"; - if (has_valid_locator(participant.metatraffic_unicast_locator)) { - meta_address = participant.metatraffic_unicast_locator.address_string(); - meta_port = static_cast(participant.metatraffic_unicast_locator.port); - } else { - meta_address = participant.address; - meta_port = participant.ports.metatraffic_unicast; - address_source = "participant.address fallback"; - } - logger_.debug( - "send_sedp -> participant {} dest {}:{} (via {}) [meta_uc_loc={}, default_uc_loc={}, " - "participant.address={}, ports.meta_uc={}]", - participant.guid_prefix.to_string(), meta_address, meta_port, address_source, - locator_to_string(participant.metatraffic_unicast_locator), - locator_to_string(participant.default_unicast_locator), participant.address, - participant.ports.metatraffic_unicast); - if (!metatraffic_unicast_receiver_ || meta_port == 0 || meta_address.empty()) { - logger_.warn("send_sedp: no usable metatraffic destination for participant {} " - "(address '{}', port {})", - participant.guid_prefix.to_string(), meta_address, meta_port); - return false; - } - - bool sent = false; - std::vector local_writers; - std::vector local_readers; - { - std::lock_guard lock(mutex_); - local_writers = writers_; - local_readers = readers_; - } - - // Each local endpoint is a stable SEDP sample: writer/reader at index i has sequence number i+1. - // Re-announcing resends the same sample under the same SN, so the SEDP HEARTBEAT range stays - // meaningful and a reliable peer can detect/recover a missed announcement. - const UdpSocket::SendConfig send_config{.ip_address = meta_address, .port = meta_port}; - for (size_t i = 0; i < local_writers.size(); i++) { - auto payload = - build_message(guid_prefix_, {.value = kSedpPublicationsReaderEntityId}, - {.value = kSedpPublicationsWriterEntityId}, static_cast(i + 1), - build_sedp_publication_payload(local_writers[i])) - .serialize(); - sent = metatraffic_unicast_receiver_->send(payload, send_config) || sent; - } - for (size_t i = 0; i < local_readers.size(); i++) { - auto payload = - build_message(guid_prefix_, {.value = kSedpSubscriptionsReaderEntityId}, - {.value = kSedpSubscriptionsWriterEntityId}, static_cast(i + 1), - build_sedp_subscription_payload(local_readers[i])) - .serialize(); - sent = metatraffic_unicast_receiver_->send(payload, send_config) || sent; - } - // HEARTBEAT our builtin SEDP writers so the peer's reliable SEDP readers ACKNACK (which lets us - // detect and retransmit a missed announcement on a lossy link). - send_sedp_heartbeats_to(meta_address, meta_port, participant.guid_prefix, local_writers.size(), - local_readers.size()); - return sent; -} - -void RtpsParticipant::send_sedp_heartbeats_to(const std::string &dest_address, uint16_t dest_port, - const GuidPrefix &dest_prefix, size_t writer_count, - size_t reader_count) { - if (!metatraffic_unicast_receiver_ || dest_address.empty() || dest_port == 0) { - return; - } - const UdpSocket::SendConfig send_config{.ip_address = dest_address, .port = dest_port}; - auto emit = [&](const std::array &reader_eid, - const std::array &writer_eid, size_t count, - std::atomic &heartbeat_count) { - if (count == 0) { - return; // nothing announced on this builtin writer yet - } - Message message; - message.header.guid_prefix = guid_prefix_; - message.submessages.push_back(build_info_dst_submessage(dest_prefix)); - message.submessages.push_back(build_heartbeat_submessage( - {.value = reader_eid}, {.value = writer_eid}, /*first_sn=*/1, - /*last_sn=*/static_cast(count), ++heartbeat_count, /*final=*/false)); - metatraffic_unicast_receiver_->send(message.serialize(), send_config); - }; - emit(kSedpPublicationsReaderEntityId, kSedpPublicationsWriterEntityId, writer_count, - sedp_pub_heartbeat_count_); - emit(kSedpSubscriptionsReaderEntityId, kSedpSubscriptionsWriterEntityId, reader_count, - sedp_sub_heartbeat_count_); -} - -std::vector -RtpsParticipant::build_directed_data_message(const GuidPrefix &dest_prefix, EntityId reader_id, - EntityId writer_id, int64_t sequence_number, - std::span serialized_payload) const { - Message message; - message.header.guid_prefix = guid_prefix_; - message.submessages.push_back(build_info_dst_submessage(dest_prefix)); - message.submessages.push_back( - {.kind = SubmessageKind::DATA, - .flags = static_cast(kSubmessageFlagLittleEndian | kSubmessageFlagData), - .payload = build_data_submessage_payload(reader_id, writer_id, sequence_number, - serialized_payload)}); - return message.serialize(); -} - -void RtpsParticipant::retransmit_user_data(const GuidPrefix &reader_prefix, EntityId reader_id, - EntityId writer_id, - const std::vector &requested_sequence_numbers) { - if (!user_unicast_receiver_ || requested_sequence_numbers.empty()) { - return; - } - // Find the local writer this ACKNACK targets (by its entity id) -> its reliable history slot. - std::optional writer_entity_index; - { - std::lock_guard lock(mutex_); - for (const auto &writer_config : writers_) { - if (writer_guid(writer_config.entity_index).entity_id == writer_id) { - writer_entity_index = writer_config.entity_index; - break; - } - } - } - if (!writer_entity_index) { - return; - } - - // Resolve where to send: the requesting reader's unicast locator, else its participant's user - // unicast endpoint. - const Guid requesting_reader_guid{.prefix = reader_prefix, .entity_id = reader_id}; - std::string dest_address; - uint16_t dest_port = 0; - for (const auto &reader : discovery_.readers()) { - if (reader.guid == requesting_reader_guid && has_valid_locator(reader.unicast_locator)) { - dest_address = reader.unicast_locator.address_string(); - dest_port = static_cast(reader.unicast_locator.port); - break; - } - } - if (dest_address.empty()) { - if (auto participant = discovery_.find_participant_by_prefix(reader_prefix)) { - dest_address = participant->address; - dest_port = participant->ports.user_unicast; - } - } - if (dest_address.empty() || dest_port == 0) { - return; - } - - // Collect the requested samples still in history, plus the lowest requested sequence number that - // has already been evicted (below the history floor) so we can GAP it. Copied under the lock; - // sent after releasing it. - std::vector>> samples; - int64_t first_available = 0; // lowest SN still in history (0 = history empty) - int64_t lowest_evicted = 0; // lowest requested SN below first_available (0 = none) - { - std::lock_guard lock(reliable_mutex_); - auto state = writer_reliable_states_.find(*writer_entity_index); - if (state == writer_reliable_states_.end()) { - return; - } - const auto &history = state->second.history; - first_available = - history.empty() ? state->second.last_sequence_number + 1 : history.begin()->first; - for (int64_t sequence_number : requested_sequence_numbers) { - auto entry = history.find(sequence_number); - if (entry != history.end()) { - samples.emplace_back(sequence_number, entry->second); - } else if (sequence_number < first_available && - (lowest_evicted == 0 || sequence_number < lowest_evicted)) { - lowest_evicted = sequence_number; - } - } - } - - const UdpSocket::SendConfig send_config{.ip_address = dest_address, .port = dest_port}; - for (const auto &[sequence_number, payload] : samples) { - auto bytes = - build_directed_data_message(reader_prefix, reader_id, writer_id, sequence_number, payload); - user_unicast_receiver_->send(bytes, send_config); - } - // Tell the reader the evicted samples are gone (GAP [lowest_evicted, first_available - 1]) so it - // advances its frontier instead of NACKing them forever. - if (lowest_evicted != 0 && first_available > lowest_evicted) { - SequenceNumberSet gap_list; - gap_list.base = first_available; // next SN the reader should expect - Message message; - message.header.guid_prefix = guid_prefix_; - message.submessages.push_back(build_info_dst_submessage(reader_prefix)); - message.submessages.push_back( - build_gap_submessage(reader_id, writer_id, lowest_evicted, gap_list)); - user_unicast_receiver_->send(message.serialize(), send_config); - } - if (!samples.empty() || lowest_evicted != 0) { - logger_.debug("Retransmitted {} user-data sample(s){} to {}:{} (writer {}, reader {})", - samples.size(), lowest_evicted != 0 ? " + GAP" : "", dest_address, dest_port, - writer_id.to_string(), reader_id.to_string()); - } -} - -void RtpsParticipant::retransmit_sedp(const GuidPrefix &reader_prefix, EntityId reader_id, - EntityId writer_id, - const std::vector &requested_sequence_numbers) { - if (!metatraffic_unicast_receiver_ || requested_sequence_numbers.empty()) { - return; - } - const bool is_publications = writer_id.value == kSedpPublicationsWriterEntityId; - const bool is_subscriptions = writer_id.value == kSedpSubscriptionsWriterEntityId; - if (!is_publications && !is_subscriptions) { - return; // not one of our builtin SEDP writers - } - - std::string dest_address; - uint16_t dest_port = 0; - if (auto participant = discovery_.find_participant_by_prefix(reader_prefix)) { - if (has_valid_locator(participant->metatraffic_unicast_locator)) { - dest_address = participant->metatraffic_unicast_locator.address_string(); - dest_port = static_cast(participant->metatraffic_unicast_locator.port); - } else { - dest_address = participant->address; - dest_port = participant->ports.metatraffic_unicast; - } - } - if (dest_address.empty() || dest_port == 0) { - return; - } - - std::vector local_writers; - std::vector local_readers; - { - std::lock_guard lock(mutex_); - local_writers = writers_; - local_readers = readers_; - } - - // Each SEDP sample's sequence number is the 1-based index of the local endpoint, so it is rebuilt - // deterministically from writers_/readers_ (no separate SEDP history cache needed). - const UdpSocket::SendConfig send_config{.ip_address = dest_address, .port = dest_port}; - size_t retransmitted = 0; - for (int64_t sequence_number : requested_sequence_numbers) { - if (sequence_number < 1) { - continue; - } - const auto index = static_cast(sequence_number - 1); - std::vector payload; - if (is_publications) { - if (index >= local_writers.size()) { - continue; - } - payload = build_sedp_publication_payload(local_writers[index]); - } else { - if (index >= local_readers.size()) { - continue; - } - payload = build_sedp_subscription_payload(local_readers[index]); - } - auto bytes = - build_directed_data_message(reader_prefix, reader_id, writer_id, sequence_number, payload); - metatraffic_unicast_receiver_->send(bytes, send_config); - retransmitted++; - } - if (retransmitted > 0) { - logger_.debug("Retransmitted {} SEDP sample(s) to {}:{} (writer {})", retransmitted, - dest_address, dest_port, writer_id.to_string()); - } -} - -void RtpsParticipant::handle_user_gap(const GuidPrefix &writer_prefix, EntityId writer_id, - int64_t gap_start, int64_t gap_list_base, - const std::vector &bitmap_irrelevant) { - const Guid writer_guid{.prefix = writer_prefix, .entity_id = writer_id}; - auto writer = discovery_.find_writer(writer_guid); - if (!writer || writer->reliability != ReliabilityKind::RELIABLE) { - return; - } - struct MatchedReader { - uint32_t entity_index{0}; - std::function)> on_sample{}; - }; - std::vector matched_readers; - { - std::lock_guard lock(mutex_); - for (const auto &reader_config : readers_) { - if (reader_config.topic_name == writer->topic_name && - reader_config.reliability == ReliabilityKind::RELIABLE && reader_config.on_sample) { - matched_readers.push_back({reader_config.entity_index, reader_config.on_sample}); - } - } - } - for (const auto &reader : matched_readers) { - std::vector> delivered; - { - std::lock_guard lock(reliable_mutex_); - auto key = fmt::format("{}#{}", reader.entity_index, writer_guid.to_string()); - apply_gap(reader_reliable_states_[key], gap_start, gap_list_base, bitmap_irrelevant, - delivered); - } - for (const auto &sample : delivered) { - reader.on_sample(sample); - } - } -} - -void RtpsParticipant::handle_builtin_gap(const GuidPrefix &writer_prefix, EntityId writer_id, - int64_t gap_start, int64_t gap_list_base, - const std::vector &bitmap_irrelevant) { - // Only the reliable builtin SEDP writers are tracked for ACKNACK accounting. - if (writer_id.value != kSedpPublicationsWriterEntityId && - writer_id.value != kSedpSubscriptionsWriterEntityId) { - return; - } - const Guid writer_guid{.prefix = writer_prefix, .entity_id = writer_id}; - std::vector> delivered; // builtin reader tracks SNs only; nothing to deliver - std::lock_guard lock(reliable_mutex_); - apply_gap(builtin_reader_states_[writer_guid.to_string()], gap_start, gap_list_base, - bitmap_irrelevant, delivered); -} - -bool RtpsParticipant::send_discovery_now() { - auto participants = discovered_participants(); - return std::accumulate(participants.begin(), participants.end(), send_spdp_announce_now(), - [this](bool sent, const auto &participant) { - return send_sedp_announcements_to(participant) || sent; - }); -} - -} // namespace espp diff --git a/components/rtps_embedded/src/rtps_participant.cpp b/components/rtps/src/rtps_participant.cpp similarity index 100% rename from components/rtps_embedded/src/rtps_participant.cpp rename to components/rtps/src/rtps_participant.cpp diff --git a/components/rtps_embedded/src/utils/Diagnostics.cpp b/components/rtps/src/utils/Diagnostics.cpp similarity index 100% rename from components/rtps_embedded/src/utils/Diagnostics.cpp rename to components/rtps/src/utils/Diagnostics.cpp diff --git a/components/rtps_embedded/CMakeLists.txt b/components/rtps_embedded/CMakeLists.txt deleted file mode 100644 index c09058553a..0000000000 --- a/components/rtps_embedded/CMakeLists.txt +++ /dev/null @@ -1,65 +0,0 @@ -idf_component_register( - SRCS - "src/rtps_participant.cpp" - "src/communication/EsppTransport.cpp" - "src/discovery/ParticipantProxyData.cpp" - "src/discovery/SEDPAgent.cpp" - "src/discovery/SPDPAgent.cpp" - "src/discovery/TopicData.cpp" - "src/entities/Domain.cpp" - "src/entities/Participant.cpp" - "src/entities/Reader.cpp" - "src/entities/StatefulReader.cpp" - "src/entities/StatefulWriter.cpp" - "src/entities/StatelessReader.cpp" - "src/entities/StatelessWriter.cpp" - "src/entities/Writer.cpp" - "src/messages/MessageReceiver.cpp" - "src/messages/MessageTypes.cpp" - "src/utils/Diagnostics.cpp" - INCLUDE_DIRS - "include" - REQUIRES - base_component cdr task thread_pool socket -) - -# Select the RTPS static-limits profile from Kconfig (see Kconfig in this -# component). The "embedded" profile is the default and is byte-identical to the -# historical behavior: it defines no RTPS_CONFIG_HEADER, so config.hpp selects -# rtps/config_esp32.hpp via ESP_PLATFORM. The relaxed "host" / "host_large" -# profiles override the profile header. These are capacity-only caps and do not -# change any bytes on the wire. -if(CONFIG_RTPS_LIMITS_PROFILE_HOST) - target_compile_definitions(${COMPONENT_LIB} PUBLIC RTPS_CONFIG_HEADER="rtps/config_desktop.hpp") -elseif(CONFIG_RTPS_LIMITS_PROFILE_HOST_LARGE) - target_compile_definitions(${COMPONENT_LIB} PUBLIC RTPS_CONFIG_HEADER="rtps/config_host_large.hpp") -endif() - -# Storage policy is orthogonal to the limits profile above: dynamic (heap, -# grow-on-full) storage is an explicit ESP opt-in (default static) so a relaxed -# 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. -if(CONFIG_RTPS_STORAGE_DYNAMIC) - target_compile_definitions(${COMPONENT_LIB} PUBLIC RTPS_STORAGE_DYNAMIC) -endif() - -# Best-effort DATA_FRAG fragmentation is opt-in on ESP targets (Kconfig, default -# off) so the MCU pays nothing for it by default. When enabled, define -# RTPS_ENABLE_FRAGMENTATION (compiles the fragment send/reassembly paths) and the -# reassembly cap RTPS_MAX_SAMPLE_SIZE (256 KB on the embedded profile). The -# facade's max payload size rises to this when fragmentation is enabled. -if(CONFIG_RTPS_ENABLE_FRAGMENTATION) - target_compile_definitions(${COMPONENT_LIB} PUBLIC - RTPS_ENABLE_FRAGMENTATION RTPS_MAX_SAMPLE_SIZE=262144) -endif() - -# The RPC layer (services + actions) is compiled in by default; the facade -# header defines RTPS_WITH_RPC unless RTPS_NO_RPC is set. When the Kconfig option -# is turned OFF, define RTPS_NO_RPC so the whole services/actions surface (and its -# std::thread/std::future use) is excluded, saving flash. ESP-only (this file is -# the ESP-IDF component build); host builds always keep RPC on. -if(NOT CONFIG_RTPS_ENABLE_RPC) - target_compile_definitions(${COMPONENT_LIB} PUBLIC RTPS_NO_RPC) -endif() - diff --git a/components/rtps_embedded/README.md b/components/rtps_embedded/README.md deleted file mode 100644 index 4d8fa567d7..0000000000 --- a/components/rtps_embedded/README.md +++ /dev/null @@ -1,156 +0,0 @@ -# rtps_embedded - -ESPP component that integrates the [embeddedRTPS](https://github.com/embedded-software-laboratory/embeddedRTPS) -RTPS/DDS stack into the ESPP ecosystem, behind an idiomatic `espp::RtpsParticipant` -facade. Any platform that can build ESPP — ESP32, Linux, macOS, Windows — can use -it to interoperate with **ROS 2** nodes (rmw_fastrtps) or any DDS participant on -the network over the standard RTPS wire protocol. - -It provides three messaging patterns, all validated against live ROS 2: - -- **Pub/sub** — topic-based, best-effort or reliable (HEARTBEAT/ACKNACK). -- **Services (RMI)** — request/reply with correlated responses. -- **Actions (AMI)** — long-running goals with feedback, result, and cancellation. - -Each has a **typed** layer (reflectable structs, no manual bytes) and a -**byte-level** layer. Services and actions come in a ROS 2-interoperable flavour -and a lean **native** (espp ↔ espp) flavour. - -The upstream embeddedRTPS library hard-depends on FreeRTOS and lwIP; -`rtps_embedded` removes those by routing all socket, task, and synchronisation -through ESPP's platform-agnostic `UdpSocket`, `Task`, `ThreadPool`, and -`SocketReactor`. On ESP32 those map to lwIP + FreeRTOS; elsewhere to the host OS. -Micro-CDR is gone — (de)serialization uses ESPP's reflection-driven `cdr`. - ---- - -## Quick-start (typed facade) - -```cpp -#include "rtps_participant.hpp" -#include "rtps_pubsub.hpp" // typed Publisher / Subscriber -#include "rtps_service.hpp" // typed ServiceServer / ServiceClient -#include "rtps_action.hpp" // typed ActionServer / ActionClient - -// Any reflectable struct is a message - fields map straight to CDR. -struct StringMsg { std::string data; }; -struct AddReq { int64_t a, b; }; -struct AddResp { int64_t sum; }; - -espp::RtpsParticipant participant({.interface_address = "192.168.1.10"}); -participant.start(); - -// Pub/sub -espp::Publisher pub(participant, {.topic = "rt/chatter", - .type_name = "std_msgs::msg::dds_::String_", - .reliability = espp::RtpsParticipant::Reliability::RELIABLE}); -espp::Subscriber sub(participant, {.topic = "rt/chatter", - .type_name = "std_msgs::msg::dds_::String_", - .on_message = [](const StringMsg &m) { /* use m.data */ }}); -pub.publish(StringMsg{"hello"}); - -// Service (RMI) - ros2 service call /add_two_ints ... hits this server -espp::ServiceServer server(participant, { - .service = "/add_two_ints", .type_name = "example_interfaces::srv::dds_::AddTwoInts", - .handler = [](const AddReq &r) { return AddResp{r.a + r.b}; }}); -espp::ServiceClient client(participant, { - .service = "/add_two_ints", .type_name = "example_interfaces::srv::dds_::AddTwoInts"}); -if (auto resp = client.call(AddReq{7, 35}, std::chrono::seconds(1))) { /* resp->sum == 42 */ } -``` - -For ROS 2 interop use ROS 2 naming: topic `rt/`, type `::msg::dds_::_`. -The full request/reply + goal APIs (including the three client call styles and the -native protocol) are documented in -[`doc/en/protocols/rtps_rmi_ami.rst`](../../doc/en/protocols/rtps_rmi_ami.rst). - -### Byte-level API - -The typed wrappers are thin layers over `espp::RtpsParticipant`'s byte-level -methods (`add_writer`/`add_reader`/`publish`, `add_service_server`/`_client`, -`add_action_server`/`_client`, and the `add_native_*` variants), which take/return -CDR-encapsulated `std::span`. Use those for dynamic types. - -Python bindings expose the same surface via the `espp` module (see -[`python/rtps_rpc_demo.py`](../../python/rtps_rpc_demo.py)). - ---- - -## Architecture - -``` -user code - │ - ▼ -espp::RtpsParticipant — the facade: start()/stop(), add_writer/reader, - │ publish, add_service_*/add_action_* - ▼ -rtps::Domain — routes packets to participants; owns discovery - │ - ├── rtps::Participant — groups writers and readers - │ ├── rtps::Writer — publishes CacheChange samples - │ └── rtps::Reader — delivers samples to a user callback - │ - └── rtps::EsppTransport — the sole platform-specific adapter: one - espp::UdpSocket per UDP port, dispatched by an - espp::SocketReactor onto a shared espp::ThreadPool - (also used for async writer work) -``` - -| Build target | Socket backend | Task backend | -|---|---|---| -| ESP32 | lwIP (via ESP-IDF) | FreeRTOS | -| Linux / macOS / PC | POSIX sockets | `std::thread` | - -Services/actions are pure library code over pub/sub — the only wire addition is -a `related_sample_identity` inline QoS on service replies (for ROS 2 correlation). - ---- - -## Configuration - -Capacity limits are chosen at build time by a **limits profile** header; storage -policy, fragmentation, and the RPC layer are separate, independent knobs. On -ESP32 these are ESP-IDF menuconfig options (`RTPS (rtps_embedded)`); on host they -default via `include/rtps/config.hpp`. - -| Knob | Options / default | Effect | -|---|---|---| -| `RTPS_LIMITS_PROFILE` | `embedded` (default) / `host` / `host_large` | Compile-time endpoint/history capacity caps (`config_esp32.hpp` / `config_desktop.hpp` / `config_host_large.hpp`). Wire-neutral. | -| `RTPS_STORAGE_DYNAMIC` | off on ESP32 / on host | Static `std::array` history (zero-heap, drop-oldest) vs heap-backed `std::deque` (grows). Orthogonal to the profile. | -| `RTPS_ENABLE_FRAGMENTATION` | off on ESP32 / on host | DATA_FRAG for samples > ~64 KB (interoperates with FastDDS/ROS 2). | -| `RTPS_ENABLE_RPC` | on (default) | Compile in services + actions (RMI/AMI). Disable to drop that code + its threads on a pure-pub/sub device. | - -The domain id, announcement/heartbeat periods, and pool sizes live in the profile -headers. - ---- - -## ESPP component dependencies - -| Component | Purpose | -|---|---| -| `base_component` | ESPP base class with integrated `espp::Logger` | -| `socket` | `UdpSocket` + `SocketReactor` used by `EsppTransport` | -| `task` | `espp::Task` / `espp::Timer` | -| `thread_pool` | shared worker pool for receive dispatch + async writer work | -| `cdr` | reflection-driven CDR/XCDR (de)serialization | - -The engine carries no vendored third-party code and has no direct dependency on -FreeRTOS, lwIP, or any platform library. - ---- - -## Example - -See [`example/`](example/) — an ESP32 (esp32-ethernet-kit) node that brings up a -participant over Ethernet and exercises the typed APIs: a `Publisher`/`Subscriber` -pair, a `ServiceServer` (`/add_two_ints`) + `ActionServer` (`/fibonacci`) a ROS 2 -client can drive, and a `ServiceClient` + `ActionClient`. A `menuconfig` option -adds a second, self-testing participant. See [`example/README.md`](example/README.md). - -## Interop & tests - -[`interop/`](interop/) runs a dockerised FastDDS / ROS 2 (Jazzy) matrix — golden -byte-for-byte wire tests, in-process loopbacks (pub/sub, services, actions, -native, typed), and live `ros2 service call` / `ros2 action send_goal` both -directions. It is gated in CI (`.github/workflows/rtps_interop.yml`). diff --git a/components/rtps_embedded/example/CMakeLists.txt b/components/rtps_embedded/example/CMakeLists.txt deleted file mode 100644 index e5c118f44b..0000000000 --- a/components/rtps_embedded/example/CMakeLists.txt +++ /dev/null @@ -1,21 +0,0 @@ -# The following lines of boilerplate have to be in your project's CMakeLists -# in this exact order for cmake to work correctly -cmake_minimum_required(VERSION 3.20) - -set(ENV{IDF_COMPONENT_MANAGER} "0") -include($ENV{IDF_PATH}/tools/cmake/project.cmake) - -set(EXTRA_COMPONENT_DIRS - "../../../components/" -) - -set( - COMPONENTS - "main esptool_py logger cdr timer rtps_embedded esp32-ethernet-kit" - CACHE STRING - "List of components to include" - ) - -project(rtps_embedded_example) - -set(CMAKE_CXX_STANDARD 20) diff --git a/components/rtps_embedded/example/README.md b/components/rtps_embedded/example/README.md deleted file mode 100644 index 3323e6134c..0000000000 --- a/components/rtps_embedded/example/README.md +++ /dev/null @@ -1,67 +0,0 @@ -# RTPS (embedded) Example - -This example brings up an `espp::RtpsParticipant` on an **ESP32-Ethernet-Kit** and -demonstrates every typed API the `rtps_embedded` component offers, interoperable -with FastDDS / ROS 2 over the standard RTPS wire protocol. - -It demonstrates: - -- Ethernet bring-up (DHCP **server** on `192.168.4.1/24`, so a directly-attached - PC gets an address) and starting a participant on the interface's IPv4 address -- a typed `Publisher` / `Subscriber` pair (reliable pub/sub) - that pairs with the FastDDS host peer in [`pc/host_pubsub.cpp`](pc/host_pubsub.cpp) -- a typed **service server** (`/add_two_ints`) and **action server** - (`/fibonacci`) the device hosts — a ROS 2 client can drive them directly with - `ros2 service call` / `ros2 action send_goal` (no manual CDR; reflectable - `AddReq`/`AddResp`, `FibGoal`/`FibSeq` structs) -- a typed **service client** + **action client** that call a peer's - `/peer_add_two_ints` / `/peer_fib` (run a ROS 2 / rclpy server for those names - to see a full round-trip; otherwise the calls simply time out, still exercising - the client API) - -All of the RMI/AMI code is compiled out when `RTPS_ENABLE_RPC` is disabled. - -## How to use example - -### Configure - -```bash -idf.py menuconfig -``` - -Under **RTPS Example Configuration**: - -| Option | Description | -|---|---| -| `RTPS_EXAMPLE_ANNOUNCE_PERIOD_MS` | Period of the outgoing publisher (default 1500 ms). | -| `RTPS_EXAMPLE_SECOND_PARTICIPANT` | Additively bring up a second, self-testing participant that calls the device's own `/add_two_ints` + `/fibonacci` for a full on-device round-trip (default off; roughly doubles the RTPS engine RAM). | - -Under **RTPS (rtps_embedded)** you can also toggle the limits profile, dynamic -storage, DATA_FRAG fragmentation, and the RPC (services + actions) layer. - -### Build and Flash - -```bash -idf.py -p PORT flash monitor -``` - -Replace `PORT` with the serial port. Connect the board's Ethernet port to a PC -(or a switch) so the participant has a network. - -### Talk to it - -- **Pub/sub host peer** (FastDDS): build and run [`pc/host_pubsub.cpp`](pc/) - against the board's topics. -- **ROS 2**: with `example_interfaces` installed and on the same network/domain: - ```bash - ros2 service call /add_two_ints example_interfaces/srv/AddTwoInts "{a: 7, b: 35}" - ros2 action send_goal -f /fibonacci example_interfaces/action/Fibonacci "{order: 5}" - ``` - -## Expected Output - -The monitor logs Ethernet link-up + the assigned IP, then `tx`/`rx` lines for the -publisher/subscriber and `service '/add_two_ints' + action '/fibonacci' ready`. -Each ROS 2 call logs the handled request/goal (e.g. `service add_two_ints: 7 + 35 -= 42`, `action fibonacci(5) done`). With the second participant enabled, `[self-test]` -lines report `PASS`/`FAIL` for the local round-trips. diff --git a/components/rtps_embedded/example/main/CMakeLists.txt b/components/rtps_embedded/example/main/CMakeLists.txt deleted file mode 100644 index 30857d2768..0000000000 --- a/components/rtps_embedded/example/main/CMakeLists.txt +++ /dev/null @@ -1,4 +0,0 @@ -idf_component_register(SRC_DIRS "." - INCLUDE_DIRS "." - REQUIRES cdr timer rtps_embedded esp32-ethernet-kit logger - ) diff --git a/components/rtps_embedded/example/main/Kconfig.projbuild b/components/rtps_embedded/example/main/Kconfig.projbuild deleted file mode 100644 index 1843ad2716..0000000000 --- a/components/rtps_embedded/example/main/Kconfig.projbuild +++ /dev/null @@ -1,25 +0,0 @@ -menu "RTPS Example Configuration" - - config RTPS_EXAMPLE_ANNOUNCE_PERIOD_MS - int "Publish period (ms)" - range 200 60000 - default 1500 - help - Period between outgoing messages published by the MCU. - - config RTPS_EXAMPLE_SECOND_PARTICIPANT - bool "Add a second (self-test) participant that calls the local servers" - default n - depends on RTPS_ENABLE_RPC - help - Additively bring up a SECOND RtpsParticipant on the device, with its - own typed ServiceClient + ActionClient, that call THIS device's own - /add_two_ints service and /fibonacci action. A participant filters out - its own messages, so this is the only way to fully round-trip the - client APIs on one device with no external peer - a self-contained - self-test. Off by default: a second participant roughly doubles the - RTPS engine's RAM (two discovery stacks, socket sets, and pools), which - may not fit on a plain ESP32 without PSRAM. Independent of the - peer-facing client demo, which always runs. - -endmenu diff --git a/components/rtps_embedded/example/main/rtps_embedded_example.cpp b/components/rtps_embedded/example/main/rtps_embedded_example.cpp deleted file mode 100644 index d39fed5e36..0000000000 --- a/components/rtps_embedded/example/main/rtps_embedded_example.cpp +++ /dev/null @@ -1,283 +0,0 @@ -// rtps_embedded component example (ESP32 / esp32-ethernet-kit). -// -// Brings up an espp::RtpsParticipant over Ethernet and demonstrates the typed -// APIs: a Publisher/Subscriber pair (pub/sub), typed ServiceServer + -// ActionServer the device hosts, and typed ServiceClient + ActionClient the -// device runs. See components/rtps_embedded/example/README.md and the RMI/AMI -// docs (doc/en/protocols/rtps_rmi_ami.rst). - -#include -#include -#include - -#include "esp32-ethernet-kit.hpp" - -#include "logger.hpp" -#include "rtps_action.hpp" -#include "rtps_participant.hpp" -#include "rtps_pubsub.hpp" -#include "rtps_service.hpp" -#include "timer.hpp" - -using namespace std::chrono_literals; - -// std_msgs/msg/String as a plain reflectable struct. The typed Publisher / -// Subscriber serialize any such struct to the DDS wire format (ROS 2 / classic -// CDR) with no manual (de)serialization in application code. -struct StringMsg { - std::string data; -}; - -// Reflectable request/reply + goal/result structs for the typed service + action -// servers below. Their fields map straight to CDR, matching example_interfaces -// so a ROS 2 client (ros2 service call / ros2 action send_goal) can drive them. -struct AddReq { - int64_t a; - int64_t b; -}; -struct AddResp { - int64_t sum; -}; -struct FibGoal { - int32_t order; -}; -struct FibSeq { - std::vector sequence; -}; - -extern "C" void app_main(void) { - espp::Logger logger({.tag = "rtps_example", .level = espp::Logger::Verbosity::INFO}); - - //! [rtps example] - // Bring up Ethernet (DHCP server on 192.168.4.1/24 so a directly-attached PC - // gets an address); any espp network interface works - the RTPS participant - // only needs the interface's IPv4 address. - auto &board = espp::Esp32EthernetKit::get(); - bool eth_ok = board.initialize_ethernet({ - .mode = espp::Esp32EthernetKit::DhcpMode::SERVER, - .on_link_up = [&]() { logger.info("Ethernet link up"); }, - .on_link_down = [&]() { logger.warn("Ethernet link down"); }, - }); - if (!eth_ok) { - logger.error("Ethernet initialization failed"); - return; - } - logger.info("Waiting for Ethernet link..."); - while (!board.is_ethernet_connected()) { - std::this_thread::sleep_for(100ms); - } - auto eth_ip = board.ethernet_ip(); - const std::string interface_address = - fmt::format("{}.{}.{}.{}", esp_ip4_addr1_16(ð_ip), esp_ip4_addr2_16(ð_ip), - esp_ip4_addr3_16(ð_ip), esp_ip4_addr4_16(ð_ip)); - logger.info("Ethernet up, IP {}", interface_address); - - // RTPS/DDS participant (embeddedRTPS engine behind the espp facade). The - // topics pair with the FastDDS host peer in example/pc/host_pubsub.cpp; for - // ROS 2 instead, use topic "rt/" with type "::msg::dds_::_" - // (e.g. "rt/chatter" + "std_msgs::msg::dds_::String_"). - constexpr const char *pub_topic = "mcu_to_pc"; - constexpr const char *sub_topic = "pc_to_mcu"; - constexpr const char *type_name = "std_msgs::msg::String"; - - // Automatic locals: they RAII-clean up in reverse order on any early return - // (subscriber/publisher stop referencing the participant before it is - // destroyed), and the trailing while(true) keeps them alive in normal use. - espp::RtpsParticipant participant({ - .interface_address = interface_address, - .on_publisher_matched = [&]() { logger.info("publisher matched a remote reader"); }, - .on_subscriber_matched = [&]() { logger.info("subscriber matched a remote writer"); }, - .log_level = espp::Logger::Verbosity::INFO, - }); - if (!participant.start()) { - logger.error("Failed to start the RTPS participant"); - return; - } - - // Typed reliable publisher: publish StringMsg structs directly (HEARTBEAT/ - // ACKNACK-acknowledged, retransmitted to matched readers). No manual CDR. - using Reliability = espp::RtpsParticipant::Reliability; - espp::Publisher publisher(participant, { - .topic = pub_topic, - .type_name = type_name, - .reliability = Reliability::RELIABLE, - }); - // Typed subscriber: receive StringMsg structs directly. - espp::Subscriber subscriber( - participant, { - .topic = sub_topic, - .type_name = type_name, - .on_message = [&](const StringMsg &msg) { logger.info("rx: {}", msg.data); }, - }); - if (!publisher.is_valid() || !subscriber.is_valid()) { - logger.error("Failed to create the typed publisher/subscriber"); - return; - } - - // Publish a counter periodically via the typed publisher. - uint32_t counter = 0; - espp::Timer publish_timer({ - .name = "rtps_pub", - .period = std::chrono::milliseconds(CONFIG_RTPS_EXAMPLE_ANNOUNCE_PERIOD_MS), - .callback = - [&]() { - if (publisher.publish(StringMsg{fmt::format("msg {}", counter++)})) { - logger.info("tx: msg {}", counter - 1); - } else { - logger.warn("tx dropped (history full)"); - } - return false; // keep the timer running - }, - .log_level = espp::Logger::Verbosity::WARN, - }); - logger.info("started: pub='{}' sub='{}' type='{}'", pub_topic, sub_topic, type_name); - -#ifdef RTPS_WITH_RPC - // Typed service (RMI) server: a ROS 2 client can `ros2 service call - // /add_two_ints example_interfaces/srv/AddTwoInts "{a: 7, b: 35}"` and get 42. - // No manual CDR - the reflectable AddReq/AddResp structs are (de)serialized for - // us. (Compiled out when CONFIG_RTPS_ENABLE_RPC is disabled.) - espp::ServiceServer add_service( - participant, { - .service = "/add_two_ints", - .type_name = "example_interfaces::srv::dds_::AddTwoInts", - .handler = - [&](const AddReq &r) { - logger.info("service add_two_ints: {} + {} = {}", r.a, r.b, r.a + r.b); - return AddResp{r.a + r.b}; - }, - }); - - // Typed action (AMI) server: a ROS 2 client can `ros2 action send_goal - // /fibonacci example_interfaces/action/Fibonacci "{order: 5}"` and receive - // feedback + the [0,1,1,2,3,5] result. execute() runs on its own thread. - espp::ActionServer fib_action( - participant, { - .action = "/fibonacci", - .type_name = "example_interfaces::action::dds_::Fibonacci", - .on_goal = [&](const FibGoal &g) { return g.order > 0; }, - .execute = - [&](auto &h) { - const int32_t order = h.goal().order; - std::vector seq{0, 1}; - for (int32_t i = 1; i < order; ++i) { - seq.push_back(seq[i] + seq[i - 1]); - h.publish_feedback(FibSeq{seq}); - std::this_thread::sleep_for(200ms); - } - h.succeed(FibSeq{seq}); - logger.info("action fibonacci({}) done", order); - }, - }); - if (!add_service.is_valid() || !fib_action.is_valid()) { - logger.error("Failed to create the typed service/action servers"); - return; - } - logger.info("service '/add_two_ints' + action '/fibonacci' ready"); - - // Also demonstrate the CLIENT side on-device: a typed service client + action - // client that call services a peer hosts ("/peer_add_two_ints", "/peer_fib"). - // Run a ROS 2 / rclpy server (or another espp device) for those names to see a - // full round-trip; until then the calls simply time out (logged), which still - // exercises the client API on-target. (Calling this device's OWN services is - // not possible - a participant filters out its own messages.) - espp::ServiceClient add_client( - participant, - {.service = "/peer_add_two_ints", .type_name = "example_interfaces::srv::dds_::AddTwoInts"}); - espp::ActionClient fib_client( - participant, - {.action = "/peer_fib", .type_name = "example_interfaces::action::dds_::Fibonacci"}); - - // Only one action goal in flight at a time: without a peer the goal never - // completes, so re-sending on every tick would leak a pending goal each time. - // The service call() below self-cleans on its 1s timeout, so it can run freely. - std::atomic fib_in_flight{false}; - espp::Timer rpc_client_timer({ - .name = "rtps_rpc_client", - .period = 5s, - .callback = - [&]() { - // Typed blocking service call (RMI). - if (auto resp = add_client.call(AddReq{20, 22}, 1s)) { - logger.info("[client] /peer_add_two_ints(20,22) = {}", resp->sum); - } else { - logger.info("[client] /peer_add_two_ints: no reply (peer serving it?)"); - } - // Typed action goal (AMI) with typed feedback + result. Skip if the - // previous goal has not finished (e.g. no peer is serving it). - if (!fib_in_flight.exchange(true)) { - fib_client.send_goal( - FibGoal{5}, [&](const FibSeq &) { /* per-feedback */ }, - [&](espp::GoalStatus status, const FibSeq &res) { - logger.info("[client] /peer_fib result: status={} len={}", - static_cast(status), res.sequence.size()); - fib_in_flight.store(false); - }); - } - return false; // keep the timer running - }, - .log_level = espp::Logger::Verbosity::WARN, - }); - if (!add_client.is_valid() || !fib_client.is_valid()) { - logger.error("Failed to create the typed service/action clients"); - return; - } - logger.info("client for '/peer_add_two_ints' + '/peer_fib' running"); - -#if CONFIG_RTPS_EXAMPLE_SECOND_PARTICIPANT - // Purely additive on-device SELF-TEST (Kconfig, default off): a SECOND - // participant with its own service + action clients that call THIS device's own - // /add_two_ints and /fibonacci servers, for a full local round-trip (a - // participant filters out its own messages, so the loopback needs a distinct - // participant). This roughly doubles the RTPS engine RAM - only enable on a - // target with headroom (e.g. PSRAM). - espp::RtpsParticipant selftest_participant({ - .interface_address = interface_address, - .log_level = espp::Logger::Verbosity::WARN, - }); - if (!selftest_participant.start()) { - logger.error("Failed to start the self-test participant"); - return; - } - espp::ServiceClient selftest_add_client( - selftest_participant, - {.service = "/add_two_ints", .type_name = "example_interfaces::srv::dds_::AddTwoInts"}); - espp::ActionClient selftest_fib_client( - selftest_participant, - {.action = "/fibonacci", .type_name = "example_interfaces::action::dds_::Fibonacci"}); - espp::Timer selftest_timer({ - .name = "rtps_selftest", - .period = 5s, - .callback = - [&]() { - if (auto resp = selftest_add_client.call(AddReq{20, 22}, 2s)) { - logger.info("[self-test] /add_two_ints(20,22) = {} ({})", resp->sum, - resp->sum == 42 ? "PASS" : "FAIL"); - } else { - logger.warn("[self-test] /add_two_ints: no reply"); - } - selftest_fib_client.send_goal( - FibGoal{5}, [&](const FibSeq &) {}, - [&](espp::GoalStatus status, const FibSeq &res) { - const std::vector expected{0, 1, 1, 2, 3, 5}; - const bool ok = status == espp::GoalStatus::SUCCEEDED && res.sequence == expected; - logger.info("[self-test] /fibonacci(5) len={} ({})", res.sequence.size(), - ok ? "PASS" : "FAIL"); - }); - return false; // keep the timer running - }, - .log_level = espp::Logger::Verbosity::WARN, - }); - if (!selftest_add_client.is_valid() || !selftest_fib_client.is_valid()) { - logger.error("Failed to create the self-test clients"); - return; - } - logger.info("self-test participant round-tripping the local service + action"); -#endif // CONFIG_RTPS_EXAMPLE_SECOND_PARTICIPANT -#endif // RTPS_WITH_RPC - //! [rtps example] - - while (true) { - std::this_thread::sleep_for(1s); - } -} diff --git a/components/rtps_embedded/example/partitions.csv b/components/rtps_embedded/example/partitions.csv deleted file mode 100644 index c4217ab9e6..0000000000 --- a/components/rtps_embedded/example/partitions.csv +++ /dev/null @@ -1,5 +0,0 @@ -# ESP-IDF Partition Table -# Name, Type, SubType, Offset, Size, Flags -nvs, data, nvs, 0x9000, 0x6000, -phy_init, data, phy, 0xf000, 0x1000, -factory, app, factory, 0x10000, 1500K, diff --git a/components/rtps_embedded/example/sdkconfig.defaults b/components/rtps_embedded/example/sdkconfig.defaults deleted file mode 100644 index 7952923ade..0000000000 --- a/components/rtps_embedded/example/sdkconfig.defaults +++ /dev/null @@ -1,28 +0,0 @@ -CONFIG_COMPILER_OPTIMIZATION_PERF=y - -# Set the FreeRTOS rate to 1ms (1000Hz) -CONFIG_FREERTOS_HZ=1000 - -CONFIG_ESP_DEFAULT_CPU_FREQ_MHZ_240=y -CONFIG_ESP_DEFAULT_CPU_FREQ_MHZ=240 - -# Common ESP-related -# -CONFIG_ESP_SYSTEM_EVENT_TASK_STACK_SIZE=4096 -CONFIG_ESP_MAIN_TASK_STACK_SIZE=8192 - -# -# Ethernet (ESP32-Ethernet-Kit A V1.2 — IP101GRI RMII PHY) -# -CONFIG_ETH_ENABLED=y -CONFIG_ETH_USE_ESP32_EMAC=y -CONFIG_ETH_PHY_USE_IP101=y - -# -# Partition Table -# -CONFIG_PARTITION_TABLE_CUSTOM=y -CONFIG_PARTITION_TABLE_CUSTOM_FILENAME="partitions.csv" -CONFIG_PARTITION_TABLE_FILENAME="partitions.csv" -CONFIG_PARTITION_TABLE_OFFSET=0x8000 -CONFIG_PARTITION_TABLE_MD5=y diff --git a/components/rtps_embedded/idf_component.yml b/components/rtps_embedded/idf_component.yml deleted file mode 100644 index 6878f70ad7..0000000000 --- a/components/rtps_embedded/idf_component.yml +++ /dev/null @@ -1,24 +0,0 @@ -## IDF Component Manager Manifest File -license: "MIT" -description: "embeddedRTPS component for ESP-IDF using espp transport and task abstractions" -url: "https://github.com/esp-cpp/espp/tree/main/components/rtps_embedded" -repository: "git://github.com/esp-cpp/espp.git" -maintainers: - - William Emfinger - - Liyun Guo -examples: - - path: example -tags: - - cpp - - RTPS - - DDS - - networking - - embedded -dependencies: - idf: - version: '>=5.0' - espp/base_component: '>=1.0' - espp/cdr: '>=1.0' - espp/task: '>=1.0' - espp/thread_pool: '>=1.0' - espp/socket: '>=1.0' diff --git a/doc/Doxyfile b/doc/Doxyfile index 6200383bab..8573631231 100755 --- a/doc/Doxyfile +++ b/doc/Doxyfile @@ -371,7 +371,11 @@ INPUT = \ $(PROJECT_PATH)/components/remote_debug/include/remote_debug.hpp \ $(PROJECT_PATH)/components/rmt/include/rmt.hpp \ $(PROJECT_PATH)/components/rmt/include/rmt_encoder.hpp \ - $(PROJECT_PATH)/components/rtps/include/rtps.hpp \ + $(PROJECT_PATH)/components/rtps/include/rtps_participant.hpp \ + $(PROJECT_PATH)/components/rtps/include/rtps_pubsub.hpp \ + $(PROJECT_PATH)/components/rtps/include/rtps_service.hpp \ + $(PROJECT_PATH)/components/rtps/include/rtps_action.hpp \ + $(PROJECT_PATH)/components/rtps/include/rtps_message.hpp \ $(PROJECT_PATH)/components/rtsp/include/generic_depacketizer.hpp \ $(PROJECT_PATH)/components/rtsp/include/generic_packetizer.hpp \ $(PROJECT_PATH)/components/rtsp/include/h264_depacketizer.hpp \ diff --git a/doc/en/protocols/rtps.rst b/doc/en/protocols/rtps.rst index 1762e44855..29e86d5a22 100644 --- a/doc/en/protocols/rtps.rst +++ b/doc/en/protocols/rtps.rst @@ -1,131 +1,182 @@ RTPS APIs ********* -The ``rtps`` component is a cross-platform foundation for building RTPS -(``Real-Time Publish-Subscribe``) functionality on top of the ESPP ``socket`` -component. - -This version now implements the first RTPS discovery layer on top of the ESPP -``socket`` component: - -- RTPS header and DATA submessage framing helpers -- standard RTPS UDPv4 port calculations -- GUID, entity ID, locator, and sequence number utility types -- SPDP participant announcements using PL_CDR parameter lists -- SEDP publication and subscription announcements for local endpoints -- parsing and tracking of discovered remote participants, writers, and readers -- integration with the shared ``cdr`` component for CDR/PL_CDR payload handling -- a participant transport layer that uses ``UdpSocket`` for metatraffic and - user-traffic channels -- optional best-effort user-data multicast transport, including endpoint-specific groups - -The long-term target is interoperability with ROS 2 nodes over DDS/RTPS, -including best-effort and reliable data flows. Discovery messages are now -standards-shaped, but full wire-compatible ROS 2 interop still needs the -remaining endpoint metadata, matching rules, and reliable RTPS state machines. - -Expected Compatibility ----------------------- - -The table below is deliberately conservative: **expected** means this is the -intended compatibility envelope of the current code, not a claim that every -peer implementation has already been validated in practice. +The ``rtps`` component is a cross-platform **RTPS / DDS** stack: it integrates the +`embeddedRTPS `_ +engine behind an idiomatic ``espp::RtpsParticipant`` facade so that any platform +that can build ESPP — ESP32, Linux, macOS, Windows — can interoperate with **ROS 2** +nodes (``rmw_fastrtps``) or any DDS participant on the network over the standard +RTPS wire protocol. -.. list-table:: - :header-rows: 1 +It provides three messaging patterns, each validated against live ROS 2 (Jazzy), +and each with a **typed** layer (reflectable structs, no manual bytes) and a +**byte-level** layer: - * - Peer implementation - - Expected compatibility - - Notes - * - ESPP ``rtps`` component / ``python/rtps_host.py`` - - **Yes** for the current scaffold - - Intended smoke-test path for SPDP, SEDP, and the temporary ``UInt32`` - ``ESPPDATA`` user-data payload. - * - Generic DDSI-RTPS 2.3 implementations - - **Partial** - - SPDP and SEDP messages are standards-shaped, but only the discovery - slice is implemented today. - * - ROS 2 nodes backed by Fast DDS - - **Partial / discovery-targeted** - - The current discovery messages include ROS 2-relevant participant user - data such as ``enclave=...;``, but standards-based ROS 2 topic data - exchange is not finished yet. - * - ROS 2 nodes backed by Cyclone DDS or other DDS vendors - - **Partial / unverified** - - Expected to be limited to the minimal discovery subset if the peer - accepts the currently emitted parameter set; not validated yet. - * - Reliable DDS/RTPS endpoints - - **No** - - ``HEARTBEAT``, ``ACKNACK``, retransmission windows, and other reliable - state-machine pieces are not implemented. - -How RTPS Works --------------- +- **Pub/sub** — topic-based, best-effort or reliable (``HEARTBEAT`` / ``ACKNACK``). +- **Services (RMI)** — request/reply with correlated responses. +- **Actions (AMI)** — long-running goals with feedback, result, and cancellation. -RTPS separates *metatraffic* from *user traffic*. +Services and actions come in a ROS 2-interoperable flavour and a lean **native** +(espp ↔ espp) flavour. The wire-format details of the RMI/AMI layers are in +:doc:`rtps_rmi_ami`. -- **Metatraffic** carries discovery and endpoint metadata. In this component, - that means SPDP participant announcements plus SEDP publication and - subscription announcements. -- **User traffic** carries application samples. The current ESPP scaffold has a - temporary best-effort ``UInt32`` user-data path while the standards-based - ROS 2 data plane is still being completed. +.. note:: -The current ``RtpsParticipant`` implementation opens three UDP sockets when -``start()`` is called: + The upstream embeddedRTPS library hard-depends on FreeRTOS and lwIP. This + component removes those, routing all socket, task, and synchronisation through + ESPP's platform-agnostic ``UdpSocket``, ``Task``, ``ThreadPool``, and + ``SocketReactor`` (lwIP + FreeRTOS on ESP32, the host OS elsewhere). Micro-CDR + is gone — (de)serialization uses ESPP's reflection-driven ``cdr``. -1. metatraffic multicast receive on the well-known SPDP multicast port -2. metatraffic unicast receive on the participant-specific discovery port -3. user unicast receive on the participant-specific user-data port +Architecture +------------ -It then starts a periodic announce task which multicasts SPDP and unicasts SEDP -endpoint announcements to each discovered peer. +The only platform-specific code is ``EsppTransport``; everything above it is +portable C++23. The ``espp::`` facade is a thin, typed surface over the ``rtps::`` +engine. .. mermaid:: - flowchart LR - App["Application code"] --> Participant["RtpsParticipant"] - Participant --> SPDP["SPDP participant DATA"] - Participant --> SEDP["SEDP publication/subscription DATA"] - Participant --> User["User DATA submessages"] - SPDP --> MetaMC["Metatraffic multicast"] - SEDP --> MetaUC["Metatraffic unicast"] - User --> UserUC["User unicast"] - MetaMC --> Peer["Remote participant"] - MetaUC --> Peer - UserUC --> Peer + flowchart TD + U["User code / ROS 2 peer"] + + subgraph facade["espp:: facade (typed + byte-level)"] + PS["Publisher / Subscriber (typed)"] + SVC["ServiceServer / ServiceClient"] + ACT["ActionServer / ActionClient"] + RP["espp::RtpsParticipant"] + PS --> RP + SVC --> RP + ACT --> RP + end + + subgraph engine["rtps:: engine (embeddedRTPS, de-vendored)"] + DOM["rtps::Domain — packet routing + discovery"] + PART["rtps::Participant"] + WR["rtps::Writer (history + HEARTBEAT)"] + RD["rtps::Reader (ACKNACK + delivery)"] + DISC["SPDP + SEDP discovery agents"] + DOM --> PART --> WR & RD + DOM --> DISC + end + + subgraph plat["platform adapter (the ONLY porting layer)"] + TR["rtps::EsppTransport"] + SOCK["espp::UdpSocket × N ports"] + REACT["espp::SocketReactor → espp::ThreadPool"] + CDR["espp::cdr (reflection CDR/XCDR)"] + TR --> SOCK --> REACT + end + + U --> facade --> engine --> plat + WR -. serialize .-> CDR + RD -. deserialize .-> CDR Discovery Flow -------------- -At a high level, discovery proceeds like this: +RTPS separates *metatraffic* (discovery) from *user traffic* (samples). A +participant announces itself with **SPDP** over multicast, then exchanges +**SEDP** endpoint metadata (topic, type, reliability, locators) with each peer. +Once a local writer and a remote reader (or vice-versa) match on topic + type, +user data flows. .. mermaid:: sequenceDiagram - participant A as Local participant - participant MC as 239.255.0.1 - participant B as Remote participant - A->>MC: SPDP DATA(participant GUID, locators, enclave, builtin endpoints) + participant A as espp participant + participant MC as 239.255.0.1 (SPDP) + participant B as ROS 2 / DDS peer + A->>MC: SPDP DATA(GUID, locators, builtin endpoints, user_data) MC-->>B: multicast delivery B->>MC: SPDP DATA(its participant metadata) MC-->>A: multicast delivery - A->>B: SEDP publication DATA(topic/type/reliability) - A->>B: SEDP subscription DATA(topic/type/reliability) + A->>B: SEDP publication/subscription DATA(topic, type, QoS, unicast locator) B->>A: SEDP publication/subscription DATA - Note over A,B: Matching user-data traffic can then use the user-unicast ports + Note over A,B: writer/reader match on topic + type → user data flows + +Pub/Sub Reliability +------------------- + +A **best-effort** writer simply sends ``DATA`` submessages; lost samples are not +recovered. A **reliable** writer keeps a history and periodically piggybacks a +``HEARTBEAT`` advertising its sequence-number range; the reader ``ACKNACK``\ s +what it is missing, and the writer retransmits. This is the state machine that +makes the component interoperate with reliable ROS 2 QoS. + +.. mermaid:: + + sequenceDiagram + participant W as Reliable Writer + participant R as Reader + W->>R: DATA (seq 1) + W-xR: DATA (seq 2) + W->>R: DATA (seq 3) + Note over W,R: seq 2 was lost + W->>R: HEARTBEAT (first=1, last=3) + R->>W: ACKNACK (missing = 2) + W->>R: DATA (seq 2) retransmit + R->>W: ACKNACK (all received) + +Large samples (> ~64 KB) are split into ``DATA_FRAG`` submessages when +``RTPS_ENABLE_FRAGMENTATION`` is on (default on host, Kconfig opt-in on ESP32); +this interoperates with FastDDS both directions. + +Services (RMI) +-------------- + +A service is two topics — a request and a reply — plus **correlation**. The +client tags its request with a ``related_sample_identity`` inline QoS carrying +its own reply-reader GUID; the server echoes ``{that GUID, the request's sequence +number}`` on the reply, and the client matches the reply back to the pending +call. This is exactly what ``rmw_fastrtps`` does, so an espp service appears in +``ros2 service list`` and answers ``ros2 service call``. + +.. mermaid:: + + sequenceDiagram + participant C as ServiceClient + participant S as ServiceServer + C->>S: request DATA (rq topic) + related_sample_identity{reply-reader GUID, seq=UNKNOWN} + Note over S: handler(request) produces response + S->>C: reply DATA (rr topic) + related_sample_identity{that GUID, request seq} + Note over C: match on {own GUID, pending seq} → deliver reply + +Clients offer three call styles: blocking ``call()``, callback ``call_async()``, +and ``call_future()`` returning a ``std::future``. -When ``handle_metatraffic_message()`` receives SPDP, it parses: +Actions (AMI) +------------- -- the remote participant GUID -- participant name -- the ``enclave=...;`` entry carried in ``PID_USER_DATA`` -- built-in endpoint bitmasks -- metatraffic and user-data locators +An action adds no new wire primitive: it composes **three services** +(``send_goal`` / ``cancel_goal`` / ``get_result``) and **two topics** +(``feedback`` / ``status``) over the RMI + pub/sub layers. A goal moves through a +small lifecycle the server drives and the client observes: -For SEDP it parses the endpoint GUID, topic name, type name, reliability, -inline-QoS expectation, and unicast locator, then updates the discovered reader -or writer cache. +.. mermaid:: + + stateDiagram-v2 + [*] --> ACCEPTED: send_goal (accepted) + [*] --> REJECTED: send_goal (rejected) + ACCEPTED --> EXECUTING: execute() starts + EXECUTING --> EXECUTING: publish_feedback() + EXECUTING --> SUCCEEDED: succeed(result) + EXECUTING --> ABORTED: abort(result) + EXECUTING --> CANCELED: cancel_goal + is_canceling() → canceled(result) + SUCCEEDED --> [*]: get_result + ABORTED --> [*]: get_result + CANCELED --> [*]: get_result + REJECTED --> [*] + +Native protocol +--------------- + +For espp ↔ espp links that do not need ROS 2 interop, a lean **native** protocol +trades interop for simplicity: services correlate with a 20-byte in-band header +(no inline QoS, no ``rq``/``rr`` mangling) on ``es_rq`` / ``es_rr`` topics, and a +native action is just a goal service + a cancel service + one feedback topic +(~4 endpoints vs the ROS action's ~10). Same client ergonomics. See +:doc:`rtps_rmi_ami` for the byte layout. Ports and Channels ------------------ @@ -151,109 +202,57 @@ The component follows the standard UDPv4 RTPS port mapping formula: - ``7400 + 250 * domain + 11 + 2 * participant`` - ``7411`` -Current ESPP Scope ------------------- - -The current implementation is intentionally focused on the first -interoperability milestone: - -- RTPS message framing and parsing -- SPDP participant discovery -- SEDP publication and subscription discovery -- participant / endpoint caches and discovery callbacks -- simple CDR little-endian serialization helpers for ``std_msgs/msg/UInt32`` - -The following pieces are **not finished yet**: +Configuration +------------- -- reliable RTPS state machines such as ``HEARTBEAT`` and ``ACKNACK`` -- standards-based ROS 2 user-data writers/readers -- full QoS matching beyond the currently emitted discovery parameters - -Feature Status --------------- +Capacity limits are chosen at build time by a **limits profile** header; storage +policy, fragmentation, and the RPC layer are separate, independent knobs (ESP-IDF +menuconfig under ``RTPS`` on ESP32; ``include/rtps/config.hpp`` defaults on host). .. list-table:: :header-rows: 1 - * - Feature - - Status - - Notes - * - RTPS header / DATA submessage serialize + parse - - **Implemented** - - Core message framing is present. - * - Standard UDPv4 RTPS port mapping - - **Implemented** - - Uses the DDSI-RTPS well-known port formula. - * - SPDP participant announce send/receive - - **Implemented** - - Multicast announce plus participant cache updates. - * - SEDP publication / subscription announce send/receive - - **Implemented** - - Local endpoints are announced and remote endpoints are cached. - * - Participant / endpoint discovery callbacks - - **Implemented** - - Exposed through ``on_participant_discovered`` and - ``on_endpoint_discovered``. - * - Temporary ``UInt32`` user-data path - - **Implemented** - - Uses the current ESPP-specific ``ESPPDATA`` payload, not a - standards-based DDS sample representation. - * - Best-effort user-data multicast transport - - **Implemented** - - Supports shared participant-level multicast or endpoint-specific - multicast locators advertised in SEDP; local readers only join the - multicast groups configured for their topics. - * - QoS fields emitted in discovery - - **Partial** - - Reliability, durability, liveliness, and history parameters are - advertised in SEDP. - * - QoS matching / policy enforcement - - **Not implemented** - - Remote QoS is parsed, but full writer/reader matching logic is still - missing. - * - Standards-based DDS user-data serialization - - **Not implemented** - - The current data path is a temporary ESPP scaffold for - ``std_msgs/msg/UInt32``. - * - Inline QoS handling - - **Not implemented** - - Discovery and user-data handling assume no inline QoS. - * - Reliable RTPS (``HEARTBEAT``, ``ACKNACK``, resend) - - **Not implemented** - - Reliable delivery is not interoperable yet. - * - Full ROS 2 topic interoperability - - **Not implemented** - - Discovery is the current milestone; ROS 2-compatible data - writers/readers are still pending. + * - Knob + - Options / default + - Effect + * - ``RTPS_LIMITS_PROFILE`` + - ``embedded`` (default) / ``host`` / ``host_large`` + - Compile-time endpoint/history capacity caps. Wire-neutral. + * - ``RTPS_STORAGE_DYNAMIC`` + - off on ESP32 / on host + - Static ``std::array`` history (zero-heap, drop-oldest) vs heap-backed + ``std::deque`` (grows). Orthogonal to the profile. + * - ``RTPS_ENABLE_FRAGMENTATION`` + - off on ESP32 / on host + - ``DATA_FRAG`` for samples > ~64 KB (interoperates with FastDDS / ROS 2). + * - ``RTPS_ENABLE_RPC`` + - on (default) + - Compile in services + actions (RMI/AMI). Disable to drop that code and its + threads on a pure-pub/sub device. Relevant Specifications ----------------------- -These are the primary standards and references for understanding the current -implementation and the remaining work: - .. list-table:: :header-rows: 1 * - Specification - Why it matters here * - `OMG DDSI-RTPS 2.3 `_ - - Primary wire-level reference for RTPS headers, DATA submessages, SPDP, - SEDP, locator encoding, GUIDs, and the UDP port mapping used by this - component. + - Primary wire-level reference for RTPS headers, submessages (``DATA``, + ``HEARTBEAT``, ``ACKNACK``, ``DATA_FRAG``), SPDP, SEDP, locator encoding, + GUIDs, and the UDP port mapping used by this component. * - `OMG DDS 1.4 `_ - - Defines the conceptual participant, reader, writer, topic, and QoS model - that RTPS discovery is advertising. + - The participant / reader / writer / topic / QoS model that RTPS carries. Example ------- -The :doc:`rtps_example` page demonstrates the current discovery scaffold by: - -- computing the RTPS ports for a participant -- building and parsing locally generated SPDP and SEDP messages -- round-tripping a ``UInt32`` value through the CDR helper functions -- registering best-effort and reliable topic endpoints in the participant API +The :doc:`rtps_example` page shows an ESP32 (esp32-ethernet-kit) node that brings +up a participant over Ethernet and exercises the typed APIs — a +``Publisher`` / ``Subscriber`` pair, a ``ServiceServer`` (``/add_two_ints``) and +``ActionServer`` (``/fibonacci``) a ROS 2 client can drive, and a +``ServiceClient`` / ``ActionClient``. .. toctree:: @@ -262,4 +261,8 @@ The :doc:`rtps_example` page demonstrates the current discovery scaffold by: API Reference ------------- -.. include-build-file:: inc/rtps.inc +.. include-build-file:: inc/rtps_participant.inc +.. include-build-file:: inc/rtps_pubsub.inc +.. include-build-file:: inc/rtps_service.inc +.. include-build-file:: inc/rtps_action.inc +.. include-build-file:: inc/rtps_message.inc diff --git a/doc/en/protocols/rtps_rmi_ami.rst b/doc/en/protocols/rtps_rmi_ami.rst index cedb8bddfc..02d56499ab 100644 --- a/doc/en/protocols/rtps_rmi_ami.rst +++ b/doc/en/protocols/rtps_rmi_ami.rst @@ -1,7 +1,7 @@ RTPS Services & Actions (RMI / AMI) *********************************** -The ``rtps_embedded`` component's :cpp:class:`espp::RtpsParticipant` facade adds +The ``rtps`` component's :cpp:class:`espp::RtpsParticipant` facade adds request/reply (**RMI** — Remote Method Invocation) and goal-oriented (**AMI** — Asynchronous Method Invocation) messaging on top of its RTPS pub/sub, in two flavours: @@ -13,7 +13,7 @@ flavours: Both flavours are *composition over the same reliable RTPS pub/sub* — no separate transport. The full design and the wire-format captures that back it are in -``components/rtps_embedded/RMI_AMI_DESIGN.md``. +``components/rtps/RMI_AMI_DESIGN.md``. Why services and actions matter =============================== @@ -228,10 +228,10 @@ Every mechanism is covered end-to-end: ``ServiceServer/Client`` + ``ActionServer/Client`` wrappers, both protocols) — plus wire-format unit tests (``rtps_service_naming``, ``rtps_action_naming``, ``rtps_action_types``) checked byte-for-byte against live ROS 2 captures. -- **On-device**: the ``components/rtps_embedded/example`` (esp32) hosts a typed +- **On-device**: the ``components/rtps/example`` (esp32) hosts a typed ``/add_two_ints`` service and a ``/fibonacci`` action a ROS 2 client can drive. - **Live ROS 2 interop** (dockerised ``rmw_fastrtps``, both directions): ``ros2 service call`` ↔ espp server, espp client ↔ rclpy server, and the same for actions (``ros2 action send_goal`` ↔ espp, espp ↔ rclpy). See - ``components/rtps_embedded/interop/``. + ``components/rtps/interop/``. - **Python**: ``python/rtps_rpc_demo.py`` exercises all four mechanisms. diff --git a/lib/espp.cmake b/lib/espp.cmake index 5f805e2a83..7bc03fa38c 100644 --- a/lib/espp.cmake +++ b/lib/espp.cmake @@ -3,9 +3,9 @@ set(ESPP_COMPONENTS "${CMAKE_CURRENT_LIST_DIR}/../components") # --------------------------------------------------------------------------- # RTPS static-limits profile selection. # -# The rtps_embedded engine keeps a fully-static (deterministic) allocation model +# The rtps engine keeps a fully-static (deterministic) allocation model # whose compile-time capacity caps are chosen by a profile header (see -# components/rtps_embedded/include/rtps/config.hpp). Profiles: +# components/rtps/include/rtps/config.hpp). Profiles: # embedded - tight MCU caps (rtps/config_esp32.hpp) # host - relaxed static caps, DEFAULT for non-ESP builds # (rtps/config_desktop.hpp) @@ -14,7 +14,7 @@ set(ESPP_COMPONENTS "${CMAKE_CURRENT_LIST_DIR}/../components") # # Select with -DRTPS_LIMITS_PROFILE=embedded|host|host_large. Default is "host" # (this file is only used for non-ESP / host builds; ESP/IDF builds pick the -# profile via Kconfig in components/rtps_embedded/Kconfig). +# profile via Kconfig in components/rtps/Kconfig). # --------------------------------------------------------------------------- if(NOT DEFINED RTPS_LIMITS_PROFILE) set(RTPS_LIMITS_PROFILE "host") @@ -47,7 +47,7 @@ message(STATUS "RTPS limits profile: ${RTPS_LIMITS_PROFILE}") # peer (interoperates with FastDDS / ROS 2). RTPS_MAX_SAMPLE_SIZE is the # large-sample reassembly cap and is kept in sync with the profile's # Config::MAX_SAMPLE_SIZE. On ESP32/IDF fragmentation is opt-in via Kconfig (see -# components/rtps_embedded/Kconfig / CMakeLists.txt), default off, so the MCU +# components/rtps/Kconfig / CMakeLists.txt), default off, so the MCU # pays nothing for it by default. # --------------------------------------------------------------------------- add_compile_definitions(RTPS_ENABLE_FRAGMENTATION RTPS_MAX_SAMPLE_SIZE=${RTPS_MAX_SAMPLE_SIZE}) @@ -89,7 +89,7 @@ set(ESPP_INCLUDES ${ESPP_COMPONENTS}/math/include ${ESPP_COMPONENTS}/ndef/include ${ESPP_COMPONENTS}/pid/include - ${ESPP_COMPONENTS}/rtps_embedded/include + ${ESPP_COMPONENTS}/rtps/include ${ESPP_COMPONENTS}/rtsp/include ${ESPP_COMPONENTS}/serialization/include ${ESPP_COMPONENTS}/tabulate/include @@ -112,23 +112,23 @@ set(ESPP_SOURCES ${ESPP_COMPONENTS}/filters/src/lowpass_filter.cpp ${ESPP_COMPONENTS}/filters/src/simple_lowpass_filter.cpp ${ESPP_COMPONENTS}/joystick/src/joystick.cpp - ${ESPP_COMPONENTS}/rtps_embedded/src/rtps_participant.cpp - ${ESPP_COMPONENTS}/rtps_embedded/src/communication/EsppTransport.cpp - ${ESPP_COMPONENTS}/rtps_embedded/src/discovery/ParticipantProxyData.cpp - ${ESPP_COMPONENTS}/rtps_embedded/src/discovery/SEDPAgent.cpp - ${ESPP_COMPONENTS}/rtps_embedded/src/discovery/SPDPAgent.cpp - ${ESPP_COMPONENTS}/rtps_embedded/src/discovery/TopicData.cpp - ${ESPP_COMPONENTS}/rtps_embedded/src/entities/Domain.cpp - ${ESPP_COMPONENTS}/rtps_embedded/src/entities/Participant.cpp - ${ESPP_COMPONENTS}/rtps_embedded/src/entities/Reader.cpp - ${ESPP_COMPONENTS}/rtps_embedded/src/entities/StatefulReader.cpp - ${ESPP_COMPONENTS}/rtps_embedded/src/entities/StatefulWriter.cpp - ${ESPP_COMPONENTS}/rtps_embedded/src/entities/StatelessReader.cpp - ${ESPP_COMPONENTS}/rtps_embedded/src/entities/StatelessWriter.cpp - ${ESPP_COMPONENTS}/rtps_embedded/src/entities/Writer.cpp - ${ESPP_COMPONENTS}/rtps_embedded/src/messages/MessageReceiver.cpp - ${ESPP_COMPONENTS}/rtps_embedded/src/messages/MessageTypes.cpp - ${ESPP_COMPONENTS}/rtps_embedded/src/utils/Diagnostics.cpp + ${ESPP_COMPONENTS}/rtps/src/rtps_participant.cpp + ${ESPP_COMPONENTS}/rtps/src/communication/EsppTransport.cpp + ${ESPP_COMPONENTS}/rtps/src/discovery/ParticipantProxyData.cpp + ${ESPP_COMPONENTS}/rtps/src/discovery/SEDPAgent.cpp + ${ESPP_COMPONENTS}/rtps/src/discovery/SPDPAgent.cpp + ${ESPP_COMPONENTS}/rtps/src/discovery/TopicData.cpp + ${ESPP_COMPONENTS}/rtps/src/entities/Domain.cpp + ${ESPP_COMPONENTS}/rtps/src/entities/Participant.cpp + ${ESPP_COMPONENTS}/rtps/src/entities/Reader.cpp + ${ESPP_COMPONENTS}/rtps/src/entities/StatefulReader.cpp + ${ESPP_COMPONENTS}/rtps/src/entities/StatefulWriter.cpp + ${ESPP_COMPONENTS}/rtps/src/entities/StatelessReader.cpp + ${ESPP_COMPONENTS}/rtps/src/entities/StatelessWriter.cpp + ${ESPP_COMPONENTS}/rtps/src/entities/Writer.cpp + ${ESPP_COMPONENTS}/rtps/src/messages/MessageReceiver.cpp + ${ESPP_COMPONENTS}/rtps/src/messages/MessageTypes.cpp + ${ESPP_COMPONENTS}/rtps/src/utils/Diagnostics.cpp ${ESPP_COMPONENTS}/rtsp/src/rtcp_packet.cpp ${ESPP_COMPONENTS}/rtsp/src/rtp_packet.cpp ${ESPP_COMPONENTS}/rtsp/src/rtsp_client.cpp diff --git a/lib/python_bindings/rtps_bindings.cpp b/lib/python_bindings/rtps_bindings.cpp index d31e592afe..fd64d2b0bb 100644 --- a/lib/python_bindings/rtps_bindings.cpp +++ b/lib/python_bindings/rtps_bindings.cpp @@ -1,5 +1,5 @@ // Hand-written pybind11 bindings for espp::RtpsParticipant (the facade over the -// embeddedRTPS engine in components/rtps_embedded — see its REFACTOR_PLAN.md). +// embeddedRTPS engine in components/rtps — see its REFACTOR_PLAN.md). // // Why hand-written (like cdr): the participant exposes std::function callbacks // taking std::span (no pybind caster) and is invoked from engine diff --git a/pc/tests/rtps_facade_frag.cpp b/pc/tests/rtps_facade_frag.cpp index d4bd9fe193..d626d2e328 100644 --- a/pc/tests/rtps_facade_frag.cpp +++ b/pc/tests/rtps_facade_frag.cpp @@ -4,7 +4,7 @@ // byte-exact against the deterministic pattern that was published. // // This is the docker-free proof of Slice C's send-split + reassembly path -// (components/rtps_embedded/REFACTOR_PLAN.md). The engine transports the raw +// (components/rtps/REFACTOR_PLAN.md). The engine transports the raw // payload bytes unchanged, so we publish an arbitrary 200 KB byte ramp (well // above the ~64 KB single-DATA limit) and assert the subscriber receives the // identical bytes. diff --git a/pc/tests/rtps_embedded_golden.cpp b/pc/tests/rtps_golden.cpp similarity index 95% rename from pc/tests/rtps_embedded_golden.cpp rename to pc/tests/rtps_golden.cpp index 8e93fb9a42..5ea166f7e4 100644 --- a/pc/tests/rtps_embedded_golden.cpp +++ b/pc/tests/rtps_golden.cpp @@ -1,6 +1,6 @@ -// Golden wire-format tests for the embeddedRTPS engine (components/rtps_embedded). +// Golden wire-format tests for the embeddedRTPS engine (components/rtps). // -// Phase 0b of components/rtps_embedded/REFACTOR_PLAN.md: freeze the engine's current +// Phase 0b of components/rtps/REFACTOR_PLAN.md: freeze the engine's current // (FastDDS/ROS2-interop-proven, Micro-CDR-based) message encodings byte-for-byte, so // codec changes (e.g. the Micro-CDR removal) and refactors can be verified to be // wire-neutral. Covers the RTPS header, INFO_DST, INFO_TS(invalid), DATA, HEARTBEAT, @@ -8,7 +8,7 @@ // PL_CDR parameter list (string alignment, PID_SENTINEL, locators) incl. round-trip. // // Regenerate goldens (only when an intentional wire change is made): -// ./rtps_embedded_golden --dump > ../tests/rtps_embedded_golden.inc +// ./rtps_golden --dump > ../tests/rtps_golden.inc // // Exits 0 when every section matches its golden byte string; 1 otherwise. @@ -207,7 +207,7 @@ bool check(const char *name, const std::vector &actual, std::span 1 && std::string(argv[1]) == "--dump") { - std::printf("// Golden wire-format byte strings for rtps_embedded_golden.cpp.\n"); - std::printf("// Generated by: ./rtps_embedded_golden --dump > " - "../tests/rtps_embedded_golden.inc\n"); + std::printf("// Golden wire-format byte strings for rtps_golden.cpp.\n"); + std::printf("// Generated by: ./rtps_golden --dump > " + "../tests/rtps_golden.inc\n"); std::printf("// Do NOT edit by hand; regenerate only on an intentional wire change.\n"); for (const auto &s : sections) { dump_array(s.name, s.build()); diff --git a/pc/tests/rtps_embedded_golden.inc b/pc/tests/rtps_golden.inc similarity index 96% rename from pc/tests/rtps_embedded_golden.inc rename to pc/tests/rtps_golden.inc index 4bbcce0e06..a18d7fa8ea 100644 --- a/pc/tests/rtps_embedded_golden.inc +++ b/pc/tests/rtps_golden.inc @@ -1,5 +1,5 @@ -// Golden wire-format byte strings for rtps_embedded_golden.cpp. -// Generated by: ./rtps_embedded_golden --dump > ../tests/rtps_embedded_golden.inc +// Golden wire-format byte strings for rtps_golden.cpp. +// Generated by: ./rtps_golden --dump > ../tests/rtps_golden.inc // Do NOT edit by hand; regenerate only on an intentional wire change. static constexpr uint8_t kGolden_header[] = { 0x52, 0x54, 0x50, 0x53, 0x02, 0x02, 0x0D, 0x25, 0x01, 0x02, 0x03, 0x04, diff --git a/pc/tests/rtps_embedded_interop_pub.cpp b/pc/tests/rtps_interop_pub.cpp similarity index 96% rename from pc/tests/rtps_embedded_interop_pub.cpp rename to pc/tests/rtps_interop_pub.cpp index 109e6faf81..f30f2c3be0 100644 --- a/pc/tests/rtps_embedded_interop_pub.cpp +++ b/pc/tests/rtps_interop_pub.cpp @@ -1,11 +1,11 @@ -// RTPS interop publisher (Phase 1 of components/rtps_embedded/REFACTOR_PLAN.md). +// RTPS interop publisher (Phase 1 of components/rtps/REFACTOR_PLAN.md). // // Exercises the espp::RtpsParticipant facade end-to-end: publishes CDR string // samples (serialized with the reflection-driven cdr::serialize) so an external DDS peer (FastDDS // or a ROS 2 node via rmw_fastrtps) can subscribe. Defaults follow the ROS 2 conventions for // std_msgs/String on /chatter. // -// Usage: rtps_embedded_interop_pub [topic] [type] [reliable(0|1)] [count] [period_ms] +// Usage: rtps_interop_pub [topic] [type] [reliable(0|1)] [count] [period_ms] // [interface_ip] [payload_bytes] // When payload_bytes > 0, publishes a String whose data is a deterministic // payload_bytes-long ASCII pattern (>64 KB exercises DATA_FRAG fragmentation); diff --git a/pc/tests/rtps_embedded_interop_sub.cpp b/pc/tests/rtps_interop_sub.cpp similarity index 94% rename from pc/tests/rtps_embedded_interop_sub.cpp rename to pc/tests/rtps_interop_sub.cpp index f9482887a4..e5723e25d9 100644 --- a/pc/tests/rtps_embedded_interop_sub.cpp +++ b/pc/tests/rtps_interop_sub.cpp @@ -1,11 +1,11 @@ -// RTPS interop subscriber (Phase 1 of components/rtps_embedded/REFACTOR_PLAN.md). +// RTPS interop subscriber (Phase 1 of components/rtps/REFACTOR_PLAN.md). // // Exercises the espp::RtpsParticipant facade end-to-end: subscribes to CDR string // samples (deserialized with the reflection-driven cdr::deserialize) published by an external DDS // peer (FastDDS or a ROS 2 node via rmw_fastrtps). Defaults follow the ROS 2 conventions for // std_msgs/String on /chatter. // -// Usage: rtps_embedded_interop_sub [topic] [type] [reliable(0|1)] [required] [timeout_s] +// Usage: rtps_interop_sub [topic] [type] [reliable(0|1)] [required] [timeout_s] // [interface_ip] [payload_bytes] // When payload_bytes > 0, each received String is verified byte-exact against the // deterministic payload_bytes-long pattern (proving fragmented >64 KB samples are @@ -29,7 +29,7 @@ struct StringMsg { std::string data; }; -// Deterministic printable pattern shared with rtps_embedded_interop_pub and the +// Deterministic printable pattern shared with rtps_interop_pub and the // ROS 2 generator in run_interop.sh (see that file). inline std::string make_pattern(std::size_t n) { std::string s(n, '\0'); diff --git a/pc/tests/rtps_native_action_loopback.cpp b/pc/tests/rtps_native_action_loopback.cpp index dd472d6387..004adbf362 100644 --- a/pc/tests/rtps_native_action_loopback.cpp +++ b/pc/tests/rtps_native_action_loopback.cpp @@ -119,7 +119,9 @@ int main() { uint8_t status2 = 0; uint32_t handle2 = 0; action->send_goal( - encode_i32(1000), // long enough (1000 * 80ms) that it can't finish before cancel + encode_i32(100), // bounded (100 * 80ms = 8s) so a lost cancel can't block stop()'s join; + // the client waits for the first feedback before canceling, so cancel + // lands mid-flight (the goal exits within ~80ms of the flag being set) [&](std::span fb) { std::lock_guard lk(m2); if (fb.size() >= 8) diff --git a/pc/tests/rtps_embedded_pubsub.cpp b/pc/tests/rtps_pubsub.cpp similarity index 97% rename from pc/tests/rtps_embedded_pubsub.cpp rename to pc/tests/rtps_pubsub.cpp index f4315d1e63..fe3eba9fea 100644 --- a/pc/tests/rtps_embedded_pubsub.cpp +++ b/pc/tests/rtps_pubsub.cpp @@ -1,6 +1,6 @@ -// Host loopback pub/sub test for the embeddedRTPS engine (components/rtps_embedded). +// Host loopback pub/sub test for the embeddedRTPS engine (components/rtps). // -// Phase 0a of components/rtps_embedded/REFACTOR_PLAN.md: establish a host-buildable, +// Phase 0a of components/rtps/REFACTOR_PLAN.md: establish a host-buildable, // runnable baseline of the engine BEFORE any refactoring, so every later phase can be // checked against it. Two participants in one Domain discover each other via SPDP/SEDP // and exchange CDR string samples (best-effort both ends; see the pool-sizing diff --git a/pc/tests/rtps_service_naming.cpp b/pc/tests/rtps_service_naming.cpp index 299fb073ef..89738efc18 100644 --- a/pc/tests/rtps_service_naming.cpp +++ b/pc/tests/rtps_service_naming.cpp @@ -1,6 +1,6 @@ // Unit test for ROS 2 service name/type mangling (rtps::rpc). The expected // strings are taken verbatim from a live rmw_fastrtps (ROS 2 Jazzy) AddTwoInts -// capture (see components/rtps_embedded/RMI_AMI_DESIGN.md 3.1/3.2). Header-only, +// capture (see components/rtps/RMI_AMI_DESIGN.md 3.1/3.2). Header-only, // no engine/runtime needed. #include diff --git a/pc/tests/rtps_typed_pubsub.cpp b/pc/tests/rtps_typed_pubsub.cpp index 9f40730389..c8f4d36d9d 100644 --- a/pc/tests/rtps_typed_pubsub.cpp +++ b/pc/tests/rtps_typed_pubsub.cpp @@ -1,4 +1,4 @@ -// Typed pub/sub test (Phase 5 of components/rtps_embedded/REFACTOR_PLAN.md). +// Typed pub/sub test (Phase 5 of components/rtps/REFACTOR_PLAN.md). // // Exercises espp::Publisher / espp::Subscriber: two facade participants in // one process exchange a reflectable message struct end-to-end, with no manual diff --git a/python/rtps_rpc_demo.py b/python/rtps_rpc_demo.py index b1089c03ba..d0cd708bd4 100644 --- a/python/rtps_rpc_demo.py +++ b/python/rtps_rpc_demo.py @@ -138,15 +138,20 @@ def count_execute(handle): nok = ndone.wait(10.0) results["native_action"] = nok and cnt["status"] == 4 and cnt["n"] == 5 and cnt["fb"] > 0 - # === 5. Native action cancel: send a long goal, cancel it mid-flight === + # === 5. Native action cancel: cancel a goal mid-flight === + # n is bounded (~6s if never canceled) so a rare lost cancel can't wall-clock + # block stop()'s join of the execute thread. Cancel triggers off the FIRST + # feedback (goal is accepted + executing, so cancel_goal() has its handle) + # rather than a fixed sleep - deterministic under CI load. ncdone = threading.Event() + nexecuting = threading.Event() ncancel = {"status": 0} nact.send_goal( - CountGoal(n=100000), # long enough (100000 * 0.03s) to cancel before it finishes - lambda f: None, + CountGoal(n=200), + lambda f: nexecuting.set(), lambda status, res: (ncancel.update(status=status), ncdone.set())) - time.sleep(0.3) # let the goal be accepted (so cancel_goal has its handle) - nact.cancel_goal() # cancels the most recently accepted goal + nexecuting.wait(5.0) # goal is executing (and accepted) -> cancel is effective + nact.cancel_goal() results["native_cancel"] = ncdone.wait(10.0) and ncancel["status"] == 5 # CANCELED server.stop()