diff --git a/.github/workflows/rtps_interop.yml b/.github/workflows/rtps_interop.yml new file mode 100644 index 0000000000..bf94870020 --- /dev/null +++ b/.github/workflows/rtps_interop.yml @@ -0,0 +1,30 @@ +name: RTPS interop (FastDDS / ROS 2) + +# Minimal token scope: the harness only checks out and builds, never writes. +permissions: + contents: read + +on: + pull_request: + paths: + - "components/rtps_embedded/**" + - "components/rtps/**" + - "components/socket/**" + - "components/cdr/**" + - "lib/espp.cmake" + - "pc/tests/rtps_embedded_*" + - ".github/workflows/rtps_interop.yml" + workflow_dispatch: + +jobs: + interop: + runs-on: ubuntu-latest + timeout-minutes: 30 + steps: + - uses: actions/checkout@v7 + with: + submodules: "recursive" + - name: Run interop matrix + run: | + cd components/rtps_embedded/interop + ./run.sh diff --git a/.gitignore b/.gitignore index de0ec4ac94..b71d71888c 100644 --- a/.gitignore +++ b/.gitignore @@ -34,6 +34,7 @@ # build folder for ESP-IDF build/ + # we only version sdkconfig.defaults sdkconfig sdkconfig.old @@ -51,3 +52,6 @@ managed_components/ # local-only agent customizations .github/agents/ + +# docker interop harness build tree (bind-mounted, built in-container) +pc/build-linux/ diff --git a/components/rtps_embedded/CMakeLists.txt b/components/rtps_embedded/CMakeLists.txt new file mode 100644 index 0000000000..c4d0b31560 --- /dev/null +++ b/components/rtps_embedded/CMakeLists.txt @@ -0,0 +1,22 @@ +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/StatelessReader.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 +) + diff --git a/components/rtps_embedded/README.md b/components/rtps_embedded/README.md new file mode 100644 index 0000000000..df92f32802 --- /dev/null +++ b/components/rtps_embedded/README.md @@ -0,0 +1,159 @@ +# rtps_embedded + +ESPP component that integrates the [embeddedRTPS](https://github.com/embedded-software-laboratory/embeddedRTPS) +RTPS/DDS stack into the ESPP ecosystem. +Any platform that can build ESPP — including ESP32, Linux, and desktop PCs — +can use this component to discover and exchange typed messages with ROS 2 nodes +or any other DDS participant on the same network using the standard RTPS wire +protocol. + +The original embeddedRTPS library has hard dependencies on FreeRTOS and lwIP. +`rtps_embedded` removes those dependencies by replacing all socket, task, and +synchronisation calls with ESPP's platform-agnostic `UdpSocket`, `Task`, and +`ThreadPool` primitives. When built for ESP32, ESPP uses FreeRTOS and lwIP +under the hood; on other platforms it uses the host OS equivalents — the RTPS +code itself is unchanged in either case. + +--- + +## Architecture + +``` +user code + │ + ▼ +rtps::Domain — routes packets to participants; owns discovery threads + │ + ├── rtps::Participant — groups writers and readers + │ ├── rtps::Writer — publishes CacheChange samples + │ └── rtps::Reader — delivers samples to a user callback + │ + ├── rtps::ThreadPool — espp::ThreadPool workers that drain the four + │ incoming/outgoing meta/user traffic queues + │ + └── rtps::EsppTransport — one espp::UdpSocket per open UDP port, + each with its own receive task +``` + +`EsppTransport` is the sole platform-specific adapter. It wraps ESPP's +`UdpSocket` and `Task`, which in turn map to: + +| Build target | Socket backend | Task backend | +|---|---|---| +| ESP32 | lwIP (via ESP-IDF) | FreeRTOS | +| Linux / PC | POSIX sockets | `std::thread` | + +--- + +## Quick-start + +```cpp +#include "rtps/entities/Domain.h" + +// 1. Construct the domain with the local interface IP. +rtps::Domain domain(local_ip); + +// 2. Create a participant *before* completeInit(). +rtps::Participant *part = domain.createParticipant(); + +// 3. Add user-defined writer and reader endpoints. +rtps::Writer *writer = domain.createWriter(*part, "my/topic", + "std_msgs::msg::String", false); +rtps::Reader *reader = domain.createReader(*part, "my/topic", + "std_msgs::msg::String", false); + +// 4. Register a receive callback on the reader. +reader->registerCallback( + [](void *, const rtps::ReaderCacheChange &change) { + // process change.getData() / change.copyInto(...) + }, nullptr); + +// 5. Start discovery (SPDP/SEDP) and worker threads. +domain.completeInit(); + +// 6. Publish a sample. +const char *payload = "hello"; +writer->newChange(rtps::ChangeKind_t::ALIVE, + reinterpret_cast(payload), + static_cast(strlen(payload) + 1)); +``` + +> **Note**: `createParticipant()` **must** be called before `completeInit()`. +> No new participants can be added after init is complete. + +--- + +## Configuration + +Two built-in config headers are provided. Select one by defining +`RTPS_CONFIG_HEADER`, or let `include/rtps/config.h` pick automatically based +on the build target. + +| Header | Target | +|---|---| +| [`include/rtps/config_esp32.h`](include/rtps/config_esp32.h) | ESP32 (ESP-IDF) | +| [`include/rtps/config_desktop.h`](include/rtps/config_desktop.h) | Linux / PC | + +All tunable constants follow the same layout in both files: + +| Constant | Default | Description | +|---|---|---| +| `DOMAIN_ID` | 0 | RTPS domain number (0–230 with UDP) | +| `MAX_NUM_PARTICIPANTS` | 1 | Participant pool size | +| `NUM_STATEFUL_WRITERS` | 5 | User writer endpoint pool | +| `NUM_STATEFUL_READERS` | 5 | User reader endpoint pool | +| `NUM_STATELESS_WRITERS` | 5 | Discovery writer endpoint pool | +| `NUM_STATELESS_READERS` | 5 | Discovery reader endpoint pool | +| `NUM_WRITERS_PER_PARTICIPANT` | 10 | Max writers per participant | +| `NUM_READERS_PER_PARTICIPANT` | 10 | Max readers per participant | +| `HISTORY_SIZE_STATEFUL` | 10 | Per-endpoint history depth | +| `THREAD_POOL_NUM_WRITERS` | 2 | Writer worker threads | +| `THREAD_POOL_NUM_READERS` | 2 | Reader worker threads | +| `THREAD_POOL_WRITER_STACKSIZE` | 4096 B | Writer task stack | +| `THREAD_POOL_READER_STACKSIZE` | 6144 B | Reader / UDP-receive task stack | +| `MAX_NUM_UDP_CONNECTIONS` | 10 | UDP socket pool size | +| `SPDP_RESEND_PERIOD_MS` | 2000 | Discovery announce period | +| `SF_WRITER_HB_PERIOD_MS` | 4000 | Reliable-writer heartbeat period | + +The `OVERALL_HEAP_SIZE` constant at the bottom of that file estimates the +total stack RAM consumed by all internal tasks. + +--- + +## ESPP component dependencies + +| Component | Purpose | +|---|---| +| `base_component` | ESPP base class with integrated `espp::Logger` | +| `socket` | ESPP `UdpSocket` used by `EsppTransport` | +| `task` | ESPP `Task` for per-port UDP receive loops | +| `thread_pool` | ESPP `ThreadPool` for writer/reader workers | +| `cdr` | CDR serialization helpers | + +These components abstract away all OS and network-stack details, so +`rtps_embedded` itself has no direct dependency on FreeRTOS, lwIP, or any +other platform library. Discovery (SPDP/SEDP) parameter-list serialization is +built on the espp `cdr` component's stream primitives (see +`include/rtps/utils/CdrBuffer.hpp`); the engine carries no vendored +third-party code. + +--- + +## Example + +See [`example/`](example/) for a two-node **initiator / responder** demo. + +The same logic runs on any ESPP-supported platform. For ESP32, flash one board +as *Initiator* and a second as *Responder* via menuconfig +(`idf.py menuconfig → RTPS Example Configuration`). The initiator periodically +publishes numbered request messages; the responder echoes each message back on +the response topic. + +Key menuconfig options (ESP32 example): + +| Option | Description | +|---|---| +| `RTPS_EXAMPLE_ROLE` | `Initiator` or `Responder` | +| `RTPS_EXAMPLE_TOPIC_PREFIX` | Shared topic prefix (e.g. `espp/rtps_example`) | +| `RTPS_EXAMPLE_PUBLISH_PERIOD_MS` | Initiator publish interval | +| `ESP_WIFI_SSID` / `ESP_WIFI_PASSWORD` | Wi-Fi credentials | diff --git a/components/rtps_embedded/REFACTOR_PLAN.md b/components/rtps_embedded/REFACTOR_PLAN.md new file mode 100644 index 0000000000..667068cf89 --- /dev/null +++ b/components/rtps_embedded/REFACTOR_PLAN.md @@ -0,0 +1,446 @@ +# RTPS Refactor: embeddedRTPS → an idiomatic espp component + +Status: IN PROGRESS — Phases 0, 1, 2, and 2b (Micro-CDR removal) complete; the +engine's entire serialization stack (user payloads and protocol parameter lists) +now runs on the reflection-driven `cdr` component and the Micro-CDR submodule is +gone. The repo moved to C++23. See the git log on feat/refactor-embedded-rtps +for per-phase commits and their verification gates. Next: Phase 3. + +## 1. Context + +espp currently carries **two** RTPS stacks: + +| | `components/rtps_embedded` | `components/rtps` | +|---|---|---| +| Origin | Vendored **embeddedRTPS** (RWTH Aachen i11, MIT) + espp glue | espp-authored clean-room implementation | +| FastDDS / ROS 2 interop | **YES — the only one that works** | **No** (aspires to, never achieved) | +| LOC | ~9,850 (+ Micro-CDR submodule) | ~3,500 | +| Architecture | Deep template/virtual hierarchy, static pools, own ThreadPool | Single 86-method class, one 2,926-line TU | +| Serialization | Micro-CDR (submodule) | espp `cdr` + hand-rolled framing | +| Wired into host lib / python | No | Yes (`lib/espp.cmake`, `rtps_bindings.cpp`) | +| Host tests | `example/pc/host_pubsub.cpp` | `pc/tests/rtps_{pubsub,publisher,subscriber}.cpp` | + +The **invariant this refactor must protect is FastDDS/ROS 2 interoperability**, and only +`rtps_embedded` has it. The native `rtps` component is itself the strongest evidence for +how this refactor must NOT be done: it is a from-scratch rewrite that replicated framing, +discovery messages, and reliable-QoS machinery — and still does not interop, because +interop lives in a long tail of wire-format and timing details that only survive by +*evolving* proven code under continuous interop testing. (Note: `components/rtps/README.md` +and `RELIABLE_RTPS_PLAN.md` overstate its status; they should be corrected or removed as +part of this work.) + +**Strategy in one sentence:** keep embeddedRTPS's proven *protocol engine* (SPDP/SEDP, +stateful writer/reader state machines, wire codec), and progressively replace everything +*around* it — threading, transport, timing, memory policy, and the user-facing API — with +espp-idiomatic infrastructure, validating FastDDS/ROS 2 interop at every phase. The native +`rtps` component is frozen, mined for its (good) API shape, python-binding and test +patterns, and finally retired; the end state is a **single component named `rtps`** with +embeddedRTPS's engine and an espp-native surface. + +## 2. Goals (from the request) + +1. Follow the style/API idioms of other espp components. +2. Simplify the class hierarchy and implementation. +3. Leverage modern C++ (concepts; C++23 where it pays — see the decision point in §7). +4. Use the cross-platform espp components effectively (`base_component`, `task`, `timer`, + `thread_pool`, `socket`/`socket_reactor`, `cdr`). +5. Improve memory and runtime efficiency of protocol work (heartbeats, announcements, + buffers). + +Non-goal: changing wire behavior. Every phase must leave FastDDS/ROS 2 interop green. + +## 3. Current architecture (`rtps_embedded`) + +### 3.1 Class hierarchy + +```mermaid +classDiagram + direction TB + class BaseComponent { <> logger } + class Domain { createParticipant() } + class Participant { addWriter() addReader() } + class Writer { <> } + class Reader { <> } + class StatelessWriterT~NetworkDriver~ + class StatefulWriterT~NetworkDriver~ + class StatelessReader + class StatefulReaderT~NetworkDriver~ + class SPDPAgent + class SEDPAgent + class MessageReceiver~NetworkDriver~ + class ThreadPool { own, not espp } + class EsppTransport { espp glue → UdpSocket } + + BaseComponent <|-- Domain + BaseComponent <|-- Participant + BaseComponent <|-- Writer + BaseComponent <|-- Reader + BaseComponent <|-- SPDPAgent + BaseComponent <|-- SEDPAgent + BaseComponent <|-- ThreadPool + Writer <|-- StatelessWriterT + Writer <|-- StatefulWriterT + Reader <|-- StatelessReader + Reader <|-- StatefulReaderT + Domain o-- Participant + Participant o-- SPDPAgent + Participant o-- SEDPAgent + Participant o-- Writer + Participant o-- Reader + Domain o-- ThreadPool + ThreadPool o-- EsppTransport +``` + +Structural issues: +- **Template-on-`NetworkDriver`** threads through writers/readers/`MessageReceiver`, + forcing `.tpp` template-implementation files and rebuilding the whole protocol per + transport — but there is exactly one transport (`EsppTransport`). This buys nothing and + costs compile time, debuggability, and readability. +- **Everything inherits `espp::BaseComponent`** (agents, pool, writers, readers). In espp + the idiom is: the *user-facing component* is a `BaseComponent`; internal helpers take a + `Logger&`/parent reference. A dozen loggers with independent tags/levels for one + participant is noise. +- **Stateless/Stateful duplication** exists both as an inheritance axis and a template + axis. + +### 3.2 Threading & timing model + +```mermaid +flowchart LR + subgraph today ["Today (per participant)"] + RX1["recv thread
SPDP multicast"] + RX2["recv thread
metatraffic unicast"] + RX3["recv thread
user unicast"] + RXN["recv thread(s)
user multicast × N"] + TPW["own ThreadPool
writer workers ×2 (queue 60)"] + TPR["own ThreadPool
reader workers ×2 (queue 60)"] + HB["heartbeat thread
sleep loop"] + SPDPT["SPDP announce thread
sleep loop"] + end +``` + +- Dedicated blocking-recv thread(s) per socket + embeddedRTPS's **own ThreadPool** (not + espp's) with fixed workload queues; plus dedicated sleep-loop threads for heartbeats and + SPDP announcements. Total: ~8+ threads/tasks with FreeRTOS stacks each. +- Heartbeats/announces are **time-driven only** — they fire on period regardless of + whether any reliable reader is behind or any data is unacknowledged. + +### 3.3 Memory model + +- `config_esp32.hpp` compile-time caps: 5 stateless + 5 stateful writers/readers, 10 + endpoints/participant, 6 proxies, history 2/10, 64-char topic/type names, 10 UDP + connections. Fixed `MemoryPool`/`ThreadSafeCircularBuffer` storage. +- Predictable footprint (good for embedded) but hard limits that a library user hits + silently, and sized-for-worst-case even when idle. + +### 3.4 What must NOT change (the interop surface) + +The protocol state machines and wire encoding that demonstrably interop with FastDDS and +ROS 2: RTPS header/submessage encoding, SPDP/SEDP parameter lists and builtin-endpoint +sets, well-known port mapping, HEARTBEAT/ACKNACK/GAP semantics and timing tolerances, +GUID/EntityId conventions, and (for ROS 2) topic/type-name conventions (`rt/…`, +`…::msg::dds_::…_`). These move between files but their behavior is frozen by tests. + +## 4. Target architecture + +### 4.1 Component layout + +One component, `components/rtps`, replacing both current components at the end state: + +``` +components/rtps/ + include/rtps.hpp # public: RtpsParticipant facade (+ typed pub/sub) + include/rtps_types.hpp # Guid, Locator, QoS enums, discovery info structs + include/detail/… # engine headers (not part of the public API) + src/participant.cpp # facade + src/engine/… # evolved embeddedRTPS core (concrete, de-templated) + example/… # esp32 example (WiFi/Ethernet pub-sub with ROS 2 notes) +``` + +### 4.2 Public API (espp idioms) + +Facade modeled on the (good) surface of the native component, per the espp canon: +`public BaseComponent`, nested `Config` with designated initializers, `std::function` +callback members, `bool` + logger error handling (the socket/protocol-family idiom), +`Task::BaseConfig` embedding, `\snippet`-wired docs. + +```cpp +namespace espp { +class RtpsParticipant : public BaseComponent { +public: + using sample_callback_t = std::function cdr_payload)>; + using participant_discovered_callback_t = std::function; + using endpoint_discovered_callback_t = std::function; + + enum class Reliability { BEST_EFFORT, RELIABLE }; + + struct WriterConfig { + std::string topic; + std::string type_name; + Reliability reliability{Reliability::BEST_EFFORT}; + size_t history_depth{10}; + std::string multicast_group{}; ///< optional user multicast + }; + struct ReaderConfig { + std::string topic; + std::string type_name; + Reliability reliability{Reliability::BEST_EFFORT}; + sample_callback_t on_sample{nullptr}; + }; + struct Config { + uint32_t domain_id{0}; + std::string interface_address{}; ///< "" → auto + std::chrono::milliseconds announce_period{1000}; + std::chrono::milliseconds heartbeat_period{200}; + Limits limits{}; ///< runtime capacity knobs (see §4.5) + std::shared_ptr reactor{nullptr}; ///< share app-wide reactor; null → own + Task::BaseConfig protocol_task_config{ + .name = "rtps", .stack_size_bytes = 6 * 1024, .priority = 10}; + participant_discovered_callback_t on_participant_discovered{nullptr}; + endpoint_discovered_callback_t on_endpoint_discovered{nullptr}; + Logger::Verbosity log_level{Logger::Verbosity::WARN}; + }; + + explicit RtpsParticipant(const Config &config); + bool start(); + void stop(); + bool is_started() const; + + // Untyped (wire-level) API — payload is a CDR-encapsulated sample: + bool add_writer(const WriterConfig &config); + bool add_reader(const ReaderConfig &config); + bool publish(std::string_view topic, std::span cdr_payload); +}; +} // namespace espp +``` + +### 4.3 Typed pub/sub via concepts (goal 3) + +A thin, header-only layer on top of the span API, using espp `cdr` and a C++20 concept — +same pattern as `TouchDriverConcept`: + +```cpp +namespace espp { +template +concept CdrSerializable = requires(const T &ct, T &t, CdrWriter &w, CdrReader &r) { + { ct.write_cdr(w) } -> std::same_as; + { t.read_cdr(r) } -> std::same_as; + { T::type_name() } -> std::convertible_to; // e.g. "std_msgs::msg::dds_::UInt32_" +}; + +template class Publisher { +public: + bool publish(const T &sample); // serializes with a reused CdrWriter, calls participant +private: + RtpsParticipant &participant_; + std::string topic_; + CdrWriter writer_; // reused buffer — no per-publish allocation +}; + +template class Subscriber { +public: + using callback_t = std::function; + // wraps ReaderConfig::on_sample with CdrReader deserialization +}; + +// helpers for ROS 2 naming so interop stays turnkey: +namespace ros2 { +std::string topic_name(std::string_view ros_topic); // "chatter" -> "rt/chatter" +} // namespace ros2 +} // namespace espp +``` + +Rationale: keeps the engine byte-oriented (interop-neutral), gives users a clean typed +API, and makes the ROS 2 naming conventions a library helper instead of user folklore. + +### 4.4 Threading model (goals 4 & 5) + +```mermaid +flowchart LR + subgraph target ["Target (per participant)"] + SR["SocketReactor
1 select() loop + shared ThreadPool
all RX sockets (3 + N multicast)"] + PT["1 protocol Timer task
deadline-scheduled:
SPDP announce · heartbeats · acknack/gap
· lease expiry"] + end + SR -->|"dispatch on pool"| ENG["engine (locked per-endpoint)"] + PT --> ENG +``` + +- **All receive sockets → `SocketReactor`** (`UdpSocket::bind()` + + `add_udp_receiver()`): 3+N blocking recv threads collapse into one select loop + the + reactor's pool. `Config::reactor` lets an application share one reactor across RTPS, + RTSP, etc. The reactor's one-shot arming preserves per-socket ordering (RTPS requires + in-order processing per locator) while different sockets process concurrently. +- **embeddedRTPS's own ThreadPool is deleted**; dispatch uses the reactor's espp + `ThreadPool`. +- **One protocol timer task replaces the heartbeat + SPDP threads**: a single + `espp::Task` waiting (cv, drift-free absolute deadlines like `espp::Timer`) on the + earliest of: next SPDP announce, next heartbeat *due*, pending acknack response delay, + participant lease checks. Two threads → one, and it sleeps to the exact next deadline. +- **Event-driven heartbeat suppression** (goal 5, per RTPS spec 8.4.2.2): heartbeats are + only scheduled while a reliable writer has unacknowledged changes for at least one + matched reader; a publish on a reliable topic piggybacks/advances the heartbeat + deadline instead of waiting for the period; a fully-acked writer goes silent. SPDP + keeps its steady cadence (that one is supposed to be periodic). + +### 4.5 Memory model (goal 5) + +- **Static pools → runtime `Limits` in `Config`** with embedded-friendly defaults + (allocated once at `start()`, not per-message): + ```cpp + struct Limits { + size_t max_writers{8}, max_readers{8}; + size_t max_remote_participants{8}, max_remote_endpoints{32}; + size_t writer_history_depth{10}, reader_reorder_depth{32}; + size_t max_message_size{1400}; // fits one UDP MTU by default + }; + ``` + Fixed-capacity behavior is preserved (reserve up front, refuse beyond limits with a + logged error) — the *predictability* of embeddedRTPS without compile-time rebuild to + change a cap. +- **Serialization buffer reuse**: per-writer and per-protocol-event scratch buffers + (`CdrWriter`/message builder with `reset()`), and in-place submessage length patching + instead of build-then-concatenate. Steady-state publish/heartbeat/announce paths do + **zero heap allocations**. +- Optional allocator hook for PSRAM placement of histories on ESP32 targets. + +### 4.6 Simplified hierarchy (goal 2) + +```mermaid +classDiagram + direction TB + class RtpsParticipant { <> } + class Transport { sockets + reactor registration } + class DiscoveryAgent { SPDP + SEDP + proxy DB (own lock) } + class DataWriterImpl { stateless|stateful by flag/QoS, own lock } + class DataReaderImpl { stateless|stateful by flag/QoS, own lock } + class MessageCodec { header/submessage encode+decode (pure, no state) } + class ProtocolTimer { deadline scheduler } + + RtpsParticipant o-- Transport + RtpsParticipant o-- DiscoveryAgent + RtpsParticipant o-- ProtocolTimer + RtpsParticipant o-- "N" DataWriterImpl + RtpsParticipant o-- "N" DataReaderImpl + DiscoveryAgent ..> MessageCodec + DataWriterImpl ..> MessageCodec + DataReaderImpl ..> MessageCodec +``` + +- **De-template**: `NetworkDriver` template parameter removed everywhere; the transport is + a concrete class. `.tpp` files fold into `.cpp`. +- **Inheritance collapses**: only `RtpsParticipant` is a `BaseComponent`. Engine classes + are concrete, `final`, own their own mutex, and take `Logger&` (or a tagged child + logger) by reference. The stateless/stateful split becomes either two concrete classes + or one class with a reliability policy — decided during Phase 4 by whichever yields + less duplication in the *proven* code (behavior-preserving transformation either way). +- **Locking**: today's cross-cutting mutexes become per-subobject locks with a documented + one-way ordering (participant → agent/endpoint), eliminating the manual 8-lock contract. + +### 4.7 Reliable exchange (behavior preserved, scheduling improved) + +```mermaid +sequenceDiagram + participant W as espp Writer (reliable) + participant R as FastDDS Reader + W->>R: DATA (seq 5) + Note over W: publish() arms heartbeat deadline
(piggyback, no fixed-period wait) + W->>R: HEARTBEAT (first..last, count) + R->>W: ACKNACK (missing {4}, count) + W->>R: DATA (seq 4) retransmit + R->>W: ACKNACK (all acked) + Note over W: writer fully acked → heartbeat
deadline cleared (silent when idle) +``` + +## 5. Migration plan (interop-gated phases) + +Each phase is a separate PR, and **must pass the Phase 0 interop gate before merge**. + +- **Phase 0 — Interop safety net (before any refactor, and before the Micro-CDR + removal merges).** + - Host-side interop harness: docker-compose with a FastDDS participant and a ROS 2 + (rmw_fastrtps) talker/listener; scripts assert bidirectional pub/sub with + `rtps_embedded`'s host build (best-effort + reliable). + - Golden wire tests: capture known-good SPDP/SEDP/DATA/HEARTBEAT/ACKNACK byte strings + from the current (Micro-CDR-based) implementation; unit-test the codec against them + byte-for-byte. Include the parameter-list corner cases a codec swap is most likely + to break: string length-prefix + null terminator + 4-byte parameter alignment, + PID_SENTINEL placement, locator encoding, GUID prefix ordering, SequenceNumberSet + bitmaps, and the per-submessage endianness (E) flag. + - CI job for the host harness; hardware smoke procedure documented for esp32. +- **Phase 1 — espp facade.** New `RtpsParticipant` facade (per §4.2) over the existing + `Domain`/`Participant` engine. Python bindings + `pc/tests` ported to the facade + (reusing the native component's binding/test patterns). No engine changes. +- **Phase 2 — Infrastructure swap.** Receive path → `SocketReactor`; embeddedRTPS + ThreadPool deleted; heartbeat/SPDP threads → single deadline-scheduled protocol task; + heartbeat suppression + publish piggyback. (Biggest efficiency win; engine state + machines untouched.) Also fix unicast port allocation: today every process starts at + participantId 0 and SO_REUSE lets a second process silently share the same unicast + ports instead of bind-failing and probing to the next participantId (found in + Phase 0c: two espp processes on one host cannot discover each other; the harness + works around it by starting the espp side first — FastDDS probes past taken ports). +- **Phase 3 — De-templating & hierarchy collapse.** Remove `NetworkDriver` template, + fold `.tpp`, concrete transport, `BaseComponent` only at the facade, per-subobject + locks. Pure mechanical/behavior-preserving; golden tests + interop gate confirm. +- **Phase 4 — Memory model.** `Limits` runtime capacities replace `config_esp32.hpp` + compile-time pools; scratch-buffer reuse; zero-alloc steady-state paths (verify with + heap tracing on esp32). +- **Phase 2b (parallel track) — Micro-CDR removal.** Underway as a parallel + exploration. Implement the §4.6 `MessageCodec` on espp `cdr` primitives plus a thin + RTPS-framing layer (submessage headers, SequenceNumber/SNSet, PL_CDR parameter + lists) and swap the engine's serialization call sites (message factory, SPDP/SEDP + proxy-data encode/decode) over to it; then delete the Micro-CDR submodule. + - **Salvage opportunity**: the native `rtps` component already contains exactly this + layer (`ByteWriter`/`ByteReader`, `ParameterView`, the full PID table, and + espp-`cdr`-based parameter-list building, ~500 LOC). Its end-to-end interop was + never proven, but under Phase 0's golden byte tests the codec layer alone can be + adopted safely — the one part of the native component worth transplanting. + - **Gating**: must not merge before Phase 0's golden tests exist — a codec swap is + precisely the class of change that breaks interop silently (alignment, sentinels, + endianness flags). + - **Sequencing**: independent of Phases 1–2 (different layers: API/infra vs codec) so + it can proceed in parallel with them, but it touches the same engine files as + Phase 3's de-templating — land it *before* Phase 3 (preferred; the fold-in then + happens with the final codec in place) rather than concurrently. +- **Phase 5 — Typed API + ROS 2 helpers.** `CdrSerializable` concept, + `Publisher`/`Subscriber`, `ros2::topic_name/type helpers`; espp `cdr` for user + payloads — after Phase 2b, facade and engine share one serialization stack. +- **Phase 6 — Consolidation.** The refactored component takes the `rtps` name; the old + native implementation and `rtps_embedded` are removed (the Micro-CDR submodule goes + with Phase 2b); `lib/espp.cmake`, python bindings, Doxyfile, build.yml, + upload_components entries updated; stale docs (`components/rtps/README.md`, + `RELIABLE_RTPS_PLAN.md`) deleted or archived into this doc's history section. + +## 6. Verification + +- **Interop matrix (the gate)**: espp↔espp (host loopback, esp32↔host), espp↔FastDDS + (both directions, best-effort + reliable), espp↔ROS 2 via rmw_fastrtps (`ros2 topic + echo` of an espp publisher; espp subscriber on a `ros2 topic pub`). +- Golden wire-format unit tests (codec byte-exactness). +- `pc/tests/*.cpp` style host tests (existing precedent) + python tests for bindings. +- esp32 measurements per phase: task count, stack usage, heap high-water mark, steady + state allocations (should reach 0 in Phase 4), CPU% at fixed pub rate, and idle network + silence when fully acked (Phase 2). + +## 7. Decision points + +1. **C++ standard — DECIDED: C++20.** Repo canon (host lib pins `cxx_std_20`; MSVC + wheels). Concepts/`span`/`requires` are established precedent and cover the goals. A + repo-wide C++23 bump can be revisited later as its own PR; the API in §4.2 does not + depend on it (bool+logger is the espp protocol-family idiom). +2. **Stateless/stateful merge shape** (two classes vs policy flag) — defer to Phase 4 + evidence. +3. **Micro-CDR retirement — DECIDED: yes, as parallel track Phase 2b** (exploration + already underway). Gated on Phase 0 golden tests; land before Phase 3. See §5. +4. **QoS surface — DECIDED: only today's proven QoS** (reliability, history depth, + multicast). Additional DDS QoS policies are added only alongside interop tests that + prove them against FastDDS/ROS 2. + +## 8. Risks + +| Risk | Mitigation | +|---|---| +| Interop regression during refactor | Phase 0 harness gates every PR; golden byte tests | +| Reactor changes RX ordering/timing | One-shot arming preserves per-socket ordering; heartbeat/acknack tolerances covered by interop reliable tests | +| Runtime limits regress embedded predictability | `Limits` reserved at `start()`; no steady-state allocation (verified by heap trace) | +| De-templating introduces subtle behavior drift | Purely mechanical phases isolated in their own PRs; golden tests byte-exact | +| Upstream embeddedRTPS divergence | We already diverged (espp glue, fixes); this refactor formalizes the fork — record provenance + license attribution in headers | diff --git a/components/rtps_embedded/example/CMakeLists.txt b/components/rtps_embedded/example/CMakeLists.txt new file mode 100644 index 0000000000..e5c118f44b --- /dev/null +++ b/components/rtps_embedded/example/CMakeLists.txt @@ -0,0 +1,21 @@ +# 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/main/CMakeLists.txt b/components/rtps_embedded/example/main/CMakeLists.txt new file mode 100644 index 0000000000..30857d2768 --- /dev/null +++ b/components/rtps_embedded/example/main/CMakeLists.txt @@ -0,0 +1,4 @@ +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 new file mode 100644 index 0000000000..ea55383598 --- /dev/null +++ b/components/rtps_embedded/example/main/Kconfig.projbuild @@ -0,0 +1,10 @@ +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. + +endmenu diff --git a/components/rtps_embedded/example/main/main.cpp b/components/rtps_embedded/example/main/main.cpp new file mode 100644 index 0000000000..88ebb155f6 --- /dev/null +++ b/components/rtps_embedded/example/main/main.cpp @@ -0,0 +1,122 @@ +#include +#include + +#include "esp32-ethernet-kit.hpp" + +#include "cdr.hpp" +#include "logger.hpp" +#include "rtps_participant.hpp" +#include "timer.hpp" + +using namespace std::chrono_literals; + +// std_msgs/msg/String: the reflection-driven cdr component serializes any +// reflectable struct straight to the DDS wire format (cdr::serialize +// emits the 4-byte encapsulation header + classic-CDR body ROS 2 speaks). +struct StringMsg { + std::string data; +}; + +// The cdr component works in std::byte; the facade publish/on_sample API uses +// uint8_t spans - bridge the two views (same bytes, different value type). +inline std::span u8_span(const std::vector &bytes) { + return {reinterpret_cast(bytes.data()), bytes.size()}; +} + +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"; + + static 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; + } + + // Reliable writer: publishes are HEARTBEAT/ACKNACK-acknowledged and + // retransmitted to matched readers. + if (!participant.add_writer({ + .topic = pub_topic, + .type_name = type_name, + .reliability = espp::RtpsParticipant::Reliability::RELIABLE, + })) { + logger.error("Failed to add the writer"); + return; + } + + // Best-effort reader: samples arrive as CDR-encapsulated bytes; decode with + // the reflection-driven cdr::deserialize. + if (!participant.add_reader({ + .topic = sub_topic, + .type_name = type_name, + .on_sample = + [&](std::span cdr_payload) { + if (auto msg = cdr::deserialize(std::as_bytes(cdr_payload)); msg) { + logger.info("rx: {}", msg->data); + } + }, + })) { + logger.error("Failed to add the reader"); + return; + } + + // Publish a counter periodically; serialization via the cdr component. + static uint32_t counter = 0; + espp::Timer publish_timer({ + .name = "rtps_pub", + .period = std::chrono::milliseconds(CONFIG_RTPS_EXAMPLE_ANNOUNCE_PERIOD_MS), + .callback = + [&]() { + auto bytes = cdr::serialize(StringMsg{fmt::format("msg {}", counter++)}); + if (bytes && participant.publish(pub_topic, u8_span(*bytes))) { + 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); + //! [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 new file mode 100644 index 0000000000..c4217ab9e6 --- /dev/null +++ b/components/rtps_embedded/example/partitions.csv @@ -0,0 +1,5 @@ +# 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/pc/CMakeLists.txt b/components/rtps_embedded/example/pc/CMakeLists.txt new file mode 100644 index 0000000000..9e94541939 --- /dev/null +++ b/components/rtps_embedded/example/pc/CMakeLists.txt @@ -0,0 +1,9 @@ +cmake_minimum_required(VERSION 3.20) +project(rtps_embedded_host_pc_example LANGUAGES CXX) + +find_package(fastdds REQUIRED COMPONENTS shared) +find_package(OpenSSL REQUIRED) + +add_executable(host_pubsub host_pubsub.cpp) +target_compile_features(host_pubsub PRIVATE cxx_std_17) +target_link_libraries(host_pubsub PRIVATE fastdds fastcdr OpenSSL::SSL OpenSSL::Crypto) diff --git a/components/rtps_embedded/example/pc/host_pubsub.cpp b/components/rtps_embedded/example/pc/host_pubsub.cpp new file mode 100644 index 0000000000..5f7c6d100d --- /dev/null +++ b/components/rtps_embedded/example/pc/host_pubsub.cpp @@ -0,0 +1,249 @@ +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace { + +using namespace eprosima::fastdds::dds; + +static constexpr uint32_t kDomainId = 0; +static constexpr uint16_t kSpdpMulticastPort = 7400; +static constexpr char kTypeName[] = "std_msgs::msg::String"; + +class RawStringPubSubType : public TopicDataType { +public: + RawStringPubSubType() { + set_name(kTypeName); + max_serialized_type_size = 256; + is_compute_key_provided = false; + } + + ~RawStringPubSubType() override = default; + + bool serialize(const void *const data, eprosima::fastdds::rtps::SerializedPayload_t &payload, + DataRepresentationId_t /*rep*/) override { + const std::string *str = static_cast(data); + const uint32_t slen = static_cast(str->size()) + 1u; + const uint32_t needed = 4u + 4u + slen; // encap header + CDR length prefix + string + null + if (needed > payload.max_size) { + return false; + } + // CDR_LE encapsulation header + payload.data[0] = 0x00; + payload.data[1] = 0x01; + payload.data[2] = 0x00; + payload.data[3] = 0x00; + // CDR string: 4-byte LE length followed by string bytes and null terminator + payload.data[4] = static_cast(slen & 0xFF); + payload.data[5] = static_cast((slen >> 8) & 0xFF); + payload.data[6] = static_cast((slen >> 16) & 0xFF); + payload.data[7] = static_cast((slen >> 24) & 0xFF); + std::memcpy(payload.data + 8, str->c_str(), slen); + payload.length = needed; + payload.encapsulation = CDR_LE; + return true; + } + + bool deserialize(eprosima::fastdds::rtps::SerializedPayload_t &payload, void *data) override { + // payload.data includes the 4-byte CDR encapsulation header followed by the CDR string body + if (payload.length < 9u) { + return false; + } + auto *str = static_cast(data); + // Skip encapsulation header, read CDR 4-byte LE string length + const uint32_t slen = static_cast(payload.data[4]) | + (static_cast(payload.data[5]) << 8) | + (static_cast(payload.data[6]) << 16) | + (static_cast(payload.data[7]) << 24); + if (slen == 0 || 8u + slen > payload.length) { + return false; + } + str->assign(reinterpret_cast(payload.data + 8), slen - 1); + return true; + } + + uint32_t calculate_serialized_size(const void *const data, + DataRepresentationId_t /*rep*/) override { + const std::string *str = static_cast(data); + return 4u + 4u + static_cast(str->size()) + 1u; + } + + bool compute_key(eprosima::fastdds::rtps::SerializedPayload_t & /*payload*/, + eprosima::fastdds::rtps::InstanceHandle_t & /*handle*/, + bool /*force_md5*/) override { + return false; + } + + bool compute_key(const void *const /*data*/, + eprosima::fastdds::rtps::InstanceHandle_t & /*handle*/, + bool /*force_md5*/) override { + return false; + } + + void *create_data() override { return new std::string(); } + + void delete_data(void *data) override { delete static_cast(data); } + + void register_type_object_representation() override {} + +#ifdef TOPIC_DATA_TYPE_API_HAS_IS_BOUNDED + bool is_bounded() const override { return false; } +#endif +#ifdef TOPIC_DATA_TYPE_API_HAS_IS_PLAIN + bool is_plain(DataRepresentationId_t) const override { return false; } +#endif +}; + +class StringListener : public DataReaderListener { +public: + explicit StringListener(std::string label) + : label_(std::move(label)) {} + + void on_data_available(DataReader *reader) override { + std::string sample; + SampleInfo info; + while (reader->take_next_sample(&sample, &info) == RETCODE_OK) { + if (info.valid_data) { + std::cout << "[rx " << label_ << "] " << sample << std::endl; + } + } + } + +private: + std::string label_; +}; + +std::atomic g_running{true}; + +void signal_handler(int) { g_running.store(false); } + +} // namespace + +int main(int argc, char **argv) { + std::signal(SIGINT, signal_handler); + std::signal(SIGTERM, signal_handler); + + std::string interface_ip = "192.168.4.2"; + int period_ms = 2000; + + for (int i = 1; i < argc; ++i) { + std::string arg = argv[i]; + if (arg == "--interface-ip" && i + 1 < argc) { + interface_ip = argv[++i]; + } else if (arg == "--period-ms" && i + 1 < argc) { + period_ms = std::stoi(argv[++i]); + } else { + std::cerr << "Usage: " << argv[0] << " [--interface-ip ] [--period-ms ]" << std::endl; + return 1; + } + } + + constexpr const char *pub_topic = "pc_to_mcu"; + constexpr const char *sub_topic = "mcu_to_pc"; + std::cout << "Publish: " << pub_topic << "\nSubscribe: " << sub_topic << std::endl; + + auto *factory = DomainParticipantFactory::get_instance(); + DomainParticipantQos qos; + factory->get_default_participant_qos(qos); + + if (!interface_ip.empty()) { + auto transport = std::make_shared(); + transport->interfaceWhiteList.push_back(interface_ip); + transport->sendBufferSize = 65536; + transport->receiveBufferSize = 65536; + qos.transport().use_builtin_transports = false; + qos.transport().user_transports.push_back(transport); + } + + eprosima::fastdds::rtps::Locator_t peer_mcast; + peer_mcast.kind = LOCATOR_KIND_UDPv4; + eprosima::fastdds::rtps::IPLocator::setIPv4(peer_mcast, "239.255.0.1"); + peer_mcast.port = kSpdpMulticastPort; + qos.wire_protocol().builtin.initialPeersList.push_back(peer_mcast); + + auto *participant = factory->create_participant(kDomainId, qos); + if (participant == nullptr) { + std::cerr << "Failed to create participant" << std::endl; + return 1; + } + + TypeSupport type_support(new RawStringPubSubType()); + if (type_support.register_type(participant) != RETCODE_OK) { + std::cerr << "Failed to register type" << std::endl; + factory->delete_participant(participant); + return 1; + } + + auto *topic_pub = + participant->create_topic(pub_topic, type_support.get_type_name(), TOPIC_QOS_DEFAULT); + auto *topic_sub = + participant->create_topic(sub_topic, type_support.get_type_name(), TOPIC_QOS_DEFAULT); + if (topic_pub == nullptr || topic_sub == nullptr) { + std::cerr << "Failed to create topics" << std::endl; + factory->delete_participant(participant); + return 1; + } + + auto *publisher = participant->create_publisher(PUBLISHER_QOS_DEFAULT); + auto *subscriber = participant->create_subscriber(SUBSCRIBER_QOS_DEFAULT); + if (publisher == nullptr || subscriber == nullptr) { + std::cerr << "Failed to create publisher/subscriber" << std::endl; + participant->delete_topic(topic_pub); + participant->delete_topic(topic_sub); + factory->delete_participant(participant); + return 1; + } + + StringListener listener(sub_topic); + auto *reader = subscriber->create_datareader(topic_sub, DATAREADER_QOS_DEFAULT, &listener); + auto *writer = publisher->create_datawriter(topic_pub, DATAWRITER_QOS_DEFAULT); + if (reader == nullptr || writer == nullptr) { + std::cerr << "Failed to create reader/writer" << std::endl; + participant->delete_subscriber(subscriber); + participant->delete_publisher(publisher); + participant->delete_topic(topic_pub); + participant->delete_topic(topic_sub); + factory->delete_participant(participant); + return 1; + } + + std::this_thread::sleep_for(std::chrono::seconds(2)); + + uint32_t counter = 0; + while (g_running.load()) { + std::string msg = "pc " + std::to_string(counter++); + if (writer->write(&msg) == RETCODE_OK) { + std::cout << "[tx] " << msg << std::endl; + } + std::this_thread::sleep_for(std::chrono::milliseconds(period_ms)); + } + + subscriber->delete_datareader(reader); + publisher->delete_datawriter(writer); + participant->delete_subscriber(subscriber); + participant->delete_publisher(publisher); + participant->delete_topic(topic_pub); + participant->delete_topic(topic_sub); + factory->delete_participant(participant); + return 0; +} diff --git a/components/rtps_embedded/example/sdkconfig.defaults b/components/rtps_embedded/example/sdkconfig.defaults new file mode 100644 index 0000000000..7952923ade --- /dev/null +++ b/components/rtps_embedded/example/sdkconfig.defaults @@ -0,0 +1,28 @@ +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 new file mode 100644 index 0000000000..6878f70ad7 --- /dev/null +++ b/components/rtps_embedded/idf_component.yml @@ -0,0 +1,24 @@ +## 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/components/rtps_embedded/include/rtps/common/Locator.hpp b/components/rtps_embedded/include/rtps/common/Locator.hpp new file mode 100644 index 0000000000..51125f7032 --- /dev/null +++ b/components/rtps_embedded/include/rtps/common/Locator.hpp @@ -0,0 +1,186 @@ +/* +The MIT License +Copyright (c) 2019 Lehrstuhl Informatik 11 - RWTH Aachen University +Modifications Copyright (c) 2026 ATDev +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: +The above copyright notice and this permission notice shall be included in +all copies or substantial portions of the Software. +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +THE SOFTWARE + +This file is part of embeddedRTPS. + +Author: i11 - Embedded Software, RWTH Aachen University +*/ + +#ifndef RTPS_LOCATOR_T_H +#define RTPS_LOCATOR_T_H + +#include "rtps/common/types.hpp" +#include "rtps/utils/CdrBuffer.hpp" +#include "rtps/utils/udpUtils.hpp" + +#include +#include + +namespace rtps { + +#if defined(_MSC_VER) +#define RTPS_EMBEDDED_PACKED +#pragma pack(push, 1) +#else +#define RTPS_EMBEDDED_PACKED __attribute__((packed)) +#endif + +inline bool isSameSubnetAddress(const std::array &addr, + const std::array &local) { + return addr[0] == local[0] && addr[1] == local[1] && addr[2] == local[2]; +} + +enum class LocatorKind_t : int32_t { + LOCATOR_KIND_INVALID = -1, + LOCATOR_KIND_RESERVED = 0, + LOCATOR_KIND_UDPv4 = 1, + LOCATOR_KIND_UDPv6 = 2 +}; + +const uint32_t LOCATOR_PORT_INVALID = 0; +const std::array LOCATOR_ADDRESS_INVALID = {0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0}; + +/* + * This representation corresponds to the RTPS wire format + */ +struct FullLengthLocator { + LocatorKind_t kind = LocatorKind_t::LOCATOR_KIND_INVALID; + uint32_t port = LOCATOR_PORT_INVALID; + std::array address = LOCATOR_ADDRESS_INVALID; // TODO make private such that kind and + // address always match? + + static FullLengthLocator createUDPv4Locator(uint8_t a, uint8_t b, uint8_t c, uint8_t d, + uint32_t port) { + FullLengthLocator locator; + locator.kind = LocatorKind_t::LOCATOR_KIND_UDPv4; + locator.address = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, a, b, c, d}; + locator.port = port; + return locator; + } + + void setInvalid() { kind = LocatorKind_t::LOCATOR_KIND_INVALID; } + + bool isValid() const { return kind != LocatorKind_t::LOCATOR_KIND_INVALID; } + + /// Reads the locator as the raw 24-byte RTPS wire representation + /// (kind int32 LE + port uint32 LE + 16 address bytes), matching this + /// struct's packed layout. + bool readFromBuffer(CdrReader &reader) { + if (reader.remaining() < sizeof(FullLengthLocator)) { + return false; + } + return readBytes(reader, reinterpret_cast(this), sizeof(FullLengthLocator)); + } + + std::array getIp4Address() const { return getIp4AddressBytes(); } + + std::array getIp4AddressBytes() const { + return {address[12], address[13], address[14], address[15]}; + } + + bool isSameAddress(const std::array &ipAddress) const { + return getIp4AddressBytes() == ipAddress; + } + + inline bool isSameSubnet(const std::array &localIp) const { + return isSameSubnetAddress(getIp4AddressBytes(), localIp); + } + + inline bool isMulticastAddress() const { + const auto ip = getIp4AddressBytes(); + return ip[0] >= 224 && ip[0] <= 239; + } + + inline uint32_t getLocatorPort() const { return static_cast(port); } + +} RTPS_EMBEDDED_PACKED; + +inline FullLengthLocator getBuiltInUnicastLocator(ParticipantId_t participantId, + const std::array &localIp) { + return FullLengthLocator::createUDPv4Locator(localIp[0], localIp[1], localIp[2], localIp[3], + getBuiltInUnicastPort(participantId)); +} + +inline FullLengthLocator getBuiltInMulticastLocator() { + return FullLengthLocator::createUDPv4Locator(239, 255, 0, 1, getBuiltInMulticastPort()); +} + +inline FullLengthLocator getUserUnicastLocator(ParticipantId_t participantId, + const std::array &localIp) { + return FullLengthLocator::createUDPv4Locator(localIp[0], localIp[1], localIp[2], localIp[3], + getUserUnicastPort(participantId)); +} + +inline FullLengthLocator +getUserMulticastLocator(const std::array &localIp) { // this would be a unicastaddress, + // as defined in config + return FullLengthLocator::createUDPv4Locator(localIp[0], localIp[1], localIp[2], localIp[3], + getUserMulticastPort()); +} + +inline FullLengthLocator getDefaultSendMulticastLocator() { + return FullLengthLocator::createUDPv4Locator(239, 255, 0, 1, getBuiltInMulticastPort()); +} + +/* + * This representation omits unnecessary 12 bytes of the full RTPS wire format + */ +struct LocatorIPv4 { + LocatorKind_t kind = LocatorKind_t::LOCATOR_KIND_INVALID; + std::array address = {0}; + uint32_t port = LOCATOR_PORT_INVALID; + + LocatorIPv4() = default; + explicit LocatorIPv4(const FullLengthLocator &locator) { + address[0] = locator.address[12]; + address[1] = locator.address[13]; + address[2] = locator.address[14]; + address[3] = locator.address[15]; + port = locator.port; + kind = locator.kind; + } + + std::array getIp4Address() const { return getIp4AddressBytes(); } + + const std::array &getIp4AddressBytes() const { return address; } + + void setInvalid() { kind = LocatorKind_t::LOCATOR_KIND_INVALID; } + + bool isValid() const { return kind != LocatorKind_t::LOCATOR_KIND_INVALID; } + + inline bool isSameSubnet(const std::array &localIp) const { + return isSameSubnetAddress(getIp4AddressBytes(), localIp); + } + + inline bool isMulticastAddress() const { + const auto ip = getIp4AddressBytes(); + return ip[0] >= 224 && ip[0] <= 239; + } +}; + +} // namespace rtps + +#if defined(_MSC_VER) +#pragma pack(pop) +#endif +#undef RTPS_EMBEDDED_PACKED + +#endif // RTPS_LOCATOR_T_H diff --git a/components/rtps_embedded/include/rtps/common/types.hpp b/components/rtps_embedded/include/rtps/common/types.hpp new file mode 100644 index 0000000000..68d1be8ca8 --- /dev/null +++ b/components/rtps_embedded/include/rtps/common/types.hpp @@ -0,0 +1,279 @@ +/* +The MIT License +Copyright (c) 2019 Lehrstuhl Informatik 11 - RWTH Aachen University +Modifications Copyright (c) 2026 ATDev +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: +The above copyright notice and this permission notice shall be included in +all copies or substantial portions of the Software. +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +THE SOFTWARE + +This file is part of embeddedRTPS. + +Author: i11 - Embedded Software, RWTH Aachen University +*/ + +#ifndef RTPS_TYPES_H +#define RTPS_TYPES_H + +#include +#include +#include +#include +#include +#include + +// TODO subnamespaces +namespace rtps { + +// TODO move types to where they are needed! + +typedef uint16_t Ip4Port_t; +typedef uint16_t DataSize_t; +typedef int8_t ParticipantId_t; // With UDP only 120 possible + +enum class EntityKind_t : uint8_t { + USER_DEFINED_UNKNOWN = 0x00, + // No user define participant + USER_DEFINED_WRITER_WITH_KEY = 0x02, + USER_DEFINED_WRITER_WITHOUT_KEY = 0x03, + USER_DEFINED_READER_WITHOUT_KEY = 0x04, + USER_DEFINED_READER_WITH_KEY = 0x07, + + BUILD_IN_UNKNOWN = 0xc0, + BUILD_IN_PARTICIPANT = 0xc1, + BUILD_IN_WRITER_WITH_KEY = 0xc2, + BUILD_IN_WRITER_WITHOUT_KEY = 0xc3, + BUILD_IN_READER_WITHOUT_KEY = 0xc4, + BUILD_IN_READER_WITH_KEY = 0xc7, + + VENDOR_SPEC_UNKNOWN = 0x40, + VENDOR_SPEC_PARTICIPANT = 0x41, + VENDOR_SPEC_WRITER_WITH_KEY = 0x42, + VENDOR_SPEC_WRITER_WITHOUT_KEY = 0x43, + VENDOR_SPEC_READER_WITHOUT_KEY = 0x44, + VENDOR_SPEC_READER_WITH_KEY = 0x47 +}; + +enum class TopicKind_t : uint8_t { NO_KEY = 1, WITH_KEY = 2 }; + +enum class ChangeKind_t : uint8_t { INVALID, ALIVE, NOT_ALIVE_DISPOSED, NOT_ALIVE_UNREGISTERED }; + +enum class ReliabilityKind_t : uint32_t { + BEST_EFFORT = 1, + RELIABLE = 2 // Specification says 3 but eprosima sends 2 +}; + +enum class DurabilityKind_t : uint32_t { + VOLATILE = 0, + TRANSIENT_LOCAL = 1, + TRANSIENT = 2, + PERSISTENT = 3 +}; + +struct GuidPrefix_t { + std::array id; + + bool operator==(const GuidPrefix_t &other) const { return this->id == other.id; } +}; + +struct EntityId_t { + std::array entityKey; + EntityKind_t entityKind; + + bool operator==(const EntityId_t &other) const { + return this->entityKey == other.entityKey && this->entityKind == other.entityKind; + } + + bool operator!=(const EntityId_t &other) const { return !(*this == other); } +}; + +struct Guid_t { + GuidPrefix_t prefix; + EntityId_t entityId; + + bool operator==(const Guid_t &other) const { + return this->prefix == other.prefix && this->entityId == other.entityId; + } + + static uint32_t sum(const Guid_t &other) { + uint32_t ret = std::accumulate(other.prefix.id.begin(), other.prefix.id.end(), uint32_t{0}); + ret = std::accumulate(other.entityId.entityKey.begin(), other.entityId.entityKey.end(), ret); + return ret; + } +}; + +// Described as long but there wasn't any definition. Other than 32 bit does not +// conform the default values +struct Time_t { + int32_t seconds; // time in seconds + uint32_t fraction; // time in sec/2^32 (?) + + static Time_t create(int32_t s, uint32_t ns) { + static constexpr double factor = (static_cast(1) << 32) / 1000000000.; + auto fraction = static_cast(ns * factor); + return Time_t{s, fraction}; + } +}; + +struct VendorId_t { + std::array vendorId; +}; + +struct SequenceNumber_t { + int32_t high; + uint32_t low; + + bool operator==(const SequenceNumber_t &other) const { + return high == other.high && low == other.low; + } + + bool operator!=(const SequenceNumber_t &other) const { return !(*this == other); } + + bool operator<(const SequenceNumber_t &other) const { + return high < other.high || (high == other.high && low < other.low); + } + + bool operator>(const SequenceNumber_t &other) const { + return high > other.high || (high == other.high && low > other.low); + } + + bool operator<=(const SequenceNumber_t &other) const { return *this == other || *this < other; } + + SequenceNumber_t &operator++() { + ++low; + if (low == 0) { + ++high; + } + return *this; + } + + SequenceNumber_t &operator--() { + if (low == 0) { + --high; + low = std::numeric_limits::max(); + } else { + --low; + } + + return *this; + } + + SequenceNumber_t operator++(int) { + SequenceNumber_t tmp(*this); + ++*this; + return tmp; + } +}; + +#define SNS_MAX_NUM_BITS 256 +#define SNS_NUM_BYTES (SNS_MAX_NUM_BITS / 8) +static_assert(!(SNS_MAX_NUM_BITS % 32) && SNS_MAX_NUM_BITS != 0, + "SNS_MAX_NUM_BITS must be multiple of 32"); + +struct SequenceNumberSet { + + SequenceNumberSet() = default; + explicit SequenceNumberSet(const SequenceNumber_t &firstMissing) + : base(firstMissing) {} + + SequenceNumber_t base = {0, 0}; + // Cannot be static because of packed + uint32_t numBits = 0; + std::array bitMap{}; + + // We only need 1 byte because atm we don't store packets. + bool isSet(uint32_t bit) const { + if (bit >= SNS_MAX_NUM_BITS) { + return true; + } + const auto bucket = static_cast(bit / 32); + const auto pos = static_cast(bit % 32); + return (bitMap[bucket] & (1 << (31 - pos))) != 0; + } +}; + +struct FragmentNumber_t { + uint32_t value; +}; + +struct Count_t { + int32_t value; +}; + +struct ProtocolVersion_t { + uint8_t major; + uint8_t minor; +}; + +typedef Time_t Duration_t; // TODO + +enum class ChangeForReaderStatusKind { UNSENT, UNACKNOWLEDGED, REQURESTED, ACKNOWLEDGED, UNDERWAY }; + +enum class ChangeFromWriterStatusKind { LOST, MISSING, RECEIVED, UNKNOWN }; + +struct InstanceHandle_t { // TODO + uint64_t value; +}; + +struct ParticipantMessageData { // TODO +}; + +using Ip4AddressBytes = std::array; + +using ReceiveCallback = void (*)(void *arg, const uint8_t *data, std::size_t size, + Ip4Port_t localPort, Ip4Port_t remotePort, + const Ip4AddressBytes &remoteAddress); + +/* Default Values */ +const EntityId_t ENTITYID_UNKNOWN{}; +const EntityId_t ENTITYID_BUILD_IN_PARTICIPANT = {{00, 00, 01}, EntityKind_t::BUILD_IN_PARTICIPANT}; +const EntityId_t ENTITYID_SEDP_BUILTIN_TOPIC_WRITER = {{00, 00, 02}, + EntityKind_t::BUILD_IN_WRITER_WITH_KEY}; +const EntityId_t ENTITYID_SEDP_BUILTIN_TOPIC_READER = {{00, 00, 02}, + EntityKind_t::BUILD_IN_READER_WITH_KEY}; +const EntityId_t ENTITYID_SEDP_BUILTIN_PUBLICATIONS_WRITER = { + {00, 00, 03}, EntityKind_t::BUILD_IN_WRITER_WITH_KEY}; +const EntityId_t ENTITYID_SEDP_BUILTIN_PUBLICATIONS_READER = { + {00, 00, 03}, EntityKind_t::BUILD_IN_READER_WITH_KEY}; +const EntityId_t ENTITYID_SEDP_BUILTIN_SUBSCRIPTIONS_WRITER = { + {00, 00, 04}, EntityKind_t::BUILD_IN_WRITER_WITH_KEY}; +const EntityId_t ENTITYID_SEDP_BUILTIN_SUBSCRIPTIONS_READER = { + {00, 00, 04}, EntityKind_t::BUILD_IN_READER_WITH_KEY}; +const EntityId_t ENTITYID_SPDP_BUILTIN_PARTICIPANT_WRITER = { + {00, 01, 00}, EntityKind_t::BUILD_IN_WRITER_WITH_KEY}; +const EntityId_t ENTITYID_SPDP_BUILTIN_PARTICIPANT_READER = { + {00, 01, 00}, EntityKind_t::BUILD_IN_READER_WITH_KEY}; +const EntityId_t ENTITYID_P2P_BUILTIN_PARTICIPANT_MESSAGE_WRITER = { + {00, 02, 00}, EntityKind_t::BUILD_IN_WRITER_WITH_KEY}; +const EntityId_t ENTITYID_P2P_BUILTIN_PARTICIPANT_MESSAGE_READER = { + {00, 02, 00}, EntityKind_t::BUILD_IN_READER_WITH_KEY}; + +const GuidPrefix_t GUIDPREFIX_UNKNOWN{}; +const Guid_t GUID_UNKNOWN{}; +const GuidPrefix_t GUID_RANDOM{9, 9, 9, 9, 9, 9, 9, 9, 9, 9, 9}; + +const ParticipantId_t PARTICIPANT_ID_INVALID = -1; + +const ProtocolVersion_t PROTOCOLVERSION = {2, 2}; + +const SequenceNumber_t SEQUENCENUMBER_UNKNOWN = {-1, 0}; + +const Time_t TIME_ZERO = {}; +const Time_t TIME_INVALID = {-1, 0xFFFFFFFF}; +const Time_t TIME_INFINITY = {0x7FFFFFFF, 0xFFFFFFFF}; + +const VendorId_t VENDOR_UNKNOWN = {}; +} // namespace rtps + +#endif // RTPS_TYPES_H diff --git a/components/rtps_embedded/include/rtps/communication/EsppTransport.hpp b/components/rtps_embedded/include/rtps/communication/EsppTransport.hpp new file mode 100644 index 0000000000..5664b4b0e0 --- /dev/null +++ b/components/rtps_embedded/include/rtps/communication/EsppTransport.hpp @@ -0,0 +1,110 @@ +/* +The MIT License +Copyright (c) 2019 Lehrstuhl Informatik 11 - RWTH Aachen University +Modifications Copyright (c) 2026 ATDev +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: +The above copyright notice and this permission notice shall be included in +all copies or substantial portions of the Software. +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +THE SOFTWARE + +This file is part of embeddedRTPS. + +Author: i11 - Embedded Software, RWTH Aachen University +*/ + +#ifndef RTPS_ESPPTRANSPORT_H +#define RTPS_ESPPTRANSPORT_H + +#include "base_component.hpp" +#include "rtps/common/types.hpp" +#include "rtps/communication/PacketInfo.hpp" +#include "rtps/config.hpp" +#include "socket_reactor.hpp" +#include "thread_pool.hpp" +#include "udp_socket.hpp" + +#include +#include +#include +#include +#include + +namespace rtps { + +class EsppTransport : public espp::BaseComponent { +public: + using RxCallback = ReceiveCallback; + + EsppTransport(RxCallback callback, void *args); + ~EsppTransport() = default; + + /// Ensure a receive channel exists for the port. Unicast ports are bound + /// with address/port reuse DISABLED so an in-use port fails loudly (the + /// Domain then probes the next participant id); multicast ports keep reuse + /// enabled so multiple processes on one host can share them. + bool ensureReceivePort(Ip4Port_t receivePort, bool is_multicast); + /// Tear down the receive channel for a port (used to unwind a partially + /// successful unicast port probe). + bool releaseReceivePort(Ip4Port_t receivePort); + bool joinMultiCastGroup(const Ip4AddressBytes &addr) const; + void sendPacket(PacketInfo &info); + + /// Submit asynchronous protocol work (e.g. a writer's progress()) onto the + /// transport's shared worker pool - the same pool the reactor dispatches + /// received datagrams on. Non-blocking; returns false (and logs) when the + /// pool queue is full or stopped. + bool submit(std::function job); + + /// Stop receive dispatch and the worker pool. Must be called before the + /// objects referenced by in-flight/queued jobs (writers, participants) are + /// destroyed; safe to call more than once. + void stop(); + +private: + struct Channel { + Ip4Port_t port{0}; + std::unique_ptr socket{}; + espp::SocketReactor::Id reactor_id{espp::SocketReactor::INVALID_ID}; + bool in_use{false}; + }; + + Channel *findChannel(Ip4Port_t port); + const Channel *findChannel(Ip4Port_t port) const; + Channel *createChannel(Ip4Port_t receivePort, bool allow_reuse); + bool startReceiver(Channel &channel, Ip4Port_t receivePort); + void onReceive(Ip4Port_t receivePort, std::vector &data, + const espp::Socket::Info &sender) const; + + static std::string ip4ToString(const Ip4AddressBytes &addr); + + RxCallback m_rxCallback{nullptr}; + void *m_callbackArgs{nullptr}; + mutable std::recursive_mutex m_mutex; + std::array m_channels{}; + /// Shared worker pool for received-datagram dispatch (via the reactor) and + /// asynchronous writer work (submit()). Declared after m_channels and before + /// m_reactor: destruction runs reactor -> pool -> channels. + std::shared_ptr m_pool{}; + /// One select() loop + a small shared worker pool multiplexes every receive + /// socket (SocketReactor's one-shot arming preserves per-socket ordering, + /// which RTPS requires per locator), replacing a dedicated blocking-recv + /// task per channel. Declared after m_channels so it is destroyed FIRST + /// (reverse member order): the reactor must stop before its sockets die. + std::shared_ptr m_reactor{}; + mutable std::vector m_multicastGroups; +}; + +} // namespace rtps + +#endif // RTPS_ESPPTRANSPORT_H diff --git a/components/rtps_embedded/include/rtps/communication/PacketInfo.hpp b/components/rtps_embedded/include/rtps/communication/PacketInfo.hpp new file mode 100644 index 0000000000..1ed8947427 --- /dev/null +++ b/components/rtps_embedded/include/rtps/communication/PacketInfo.hpp @@ -0,0 +1,64 @@ +/* +The MIT License +Copyright (c) 2019 Lehrstuhl Informatik 11 - RWTH Aachen University +Modifications Copyright (c) 2026 ATDev +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: +The above copyright notice and this permission notice shall be included in +all copies or substantial portions of the Software. +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +THE SOFTWARE + +This file is part of embeddedRTPS. + +Author: i11 - Embedded Software, RWTH Aachen University +*/ + +// Copyright 2023 Apex.AI, Inc. +// All rights reserved. + +#ifndef RTPS_PACKETINFO_H +#define RTPS_PACKETINFO_H + +#include +#include + +#include "rtps/common/types.hpp" + +namespace rtps { + +struct PacketInfo { + Ip4Port_t srcPort; // TODO Do we need that? + std::array destAddr = {0, 0, 0, 0}; + Ip4Port_t destPort; + std::vector payload; + + void copyTriviallyCopyable(const PacketInfo &other) { + this->srcPort = other.srcPort; + this->destPort = other.destPort; + this->destAddr = other.destAddr; + } + + PacketInfo() = default; + ~PacketInfo() = default; + + PacketInfo &operator=(const PacketInfo &other) = delete; + + PacketInfo &operator=(PacketInfo &&other) noexcept { + copyTriviallyCopyable(other); + this->payload = std::move(other.payload); + return *this; + } +}; +} // namespace rtps + +#endif // RTPS_PACKETINFO_H diff --git a/components/rtps_embedded/include/rtps/config.hpp b/components/rtps_embedded/include/rtps/config.hpp new file mode 100644 index 0000000000..43abe37905 --- /dev/null +++ b/components/rtps_embedded/include/rtps/config.hpp @@ -0,0 +1,39 @@ +/* +The MIT License +Copyright (c) 2019 Lehrstuhl Informatik 11 - RWTH Aachen University +Modifications Copyright (c) 2026 ATDev +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: +The above copyright notice and this permission notice shall be included in +all copies or substantial portions of the Software. +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +THE SOFTWARE + +This file is part of embeddedRTPS. + +Author: i11 - Embedded Software, RWTH Aachen University +*/ + +#ifndef RTPS_CONFIG_H +#define RTPS_CONFIG_H + +#ifdef RTPS_CONFIG_HEADER +#include RTPS_CONFIG_HEADER +#else +#if defined(ESP_PLATFORM) +#include "rtps/config_esp32.hpp" +#else +#include "rtps/config_desktop.hpp" +#endif +#endif + +#endif // RTPS_CONFIG_H diff --git a/components/rtps_embedded/include/rtps/config_desktop.hpp b/components/rtps_embedded/include/rtps/config_desktop.hpp new file mode 100644 index 0000000000..f480c72fb4 --- /dev/null +++ b/components/rtps_embedded/include/rtps/config_desktop.hpp @@ -0,0 +1,100 @@ +/* +The MIT License +Copyright (c) 2019 Lehrstuhl Informatik 11 - RWTH Aachen University +Modifications Copyright (c) 2026 ATDev +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: +The above copyright notice and this permission notice shall be included in +all copies or substantial portions of the Software. +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +THE SOFTWARE + +This file is part of embeddedRTPS. + +Author: i11 - Embedded Software, RWTH Aachen University +*/ + +#ifndef RTPS_CONFIG_DESKTOP_H +#define RTPS_CONFIG_DESKTOP_H + +#include "rtps/common/types.hpp" + +namespace rtps { + +#define IS_LITTLE_ENDIAN 1 + +namespace Config { +const VendorId_t VENDOR_ID = {13, 37}; +const std::array IP_ADDRESS = {192, 168, 4, 1}; // Needs to be set in lwipcfg.h too. +// GUID_RANDOM: derive each participant prefix from OS entropy (see +// Domain::generateGuidPrefix). A fixed prefix here makes every desktop +// participant share an identity, which breaks discovery between them. +const GuidPrefix_t BASE_GUID_PREFIX = GUID_RANDOM; + +const uint8_t DOMAIN_ID = 0; // 230 possible with UDP +const uint8_t MAX_NUM_PARTICIPANTS = 2; +const uint8_t NUM_STATELESS_WRITERS = MAX_NUM_PARTICIPANTS + 1; // Required + Additional +const uint8_t NUM_STATELESS_READERS = MAX_NUM_PARTICIPANTS + 1; // Required + Additional +const uint8_t NUM_STATEFUL_READERS = 4; // 1-4 required per participant depending on what they do + // and to whom they match +const uint8_t NUM_STATEFUL_WRITERS = 4; // 1-4 required per participant depending on what they do + // and to whom they match +const uint8_t NUM_WRITERS_PER_PARTICIPANT = 4; +const uint8_t NUM_READERS_PER_PARTICIPANT = 4; +const uint8_t NUM_WRITER_PROXIES_PER_READER = 3; +const uint8_t NUM_READER_PROXIES_PER_WRITER = 3; + +const uint8_t MAX_NUM_UNMATCHED_REMOTE_WRITERS = 100; +const uint8_t MAX_NUM_UNMATCHED_REMOTE_READERS = 10; + +const uint8_t MAX_NUM_READER_CALLBACKS = 5; + +const uint8_t HISTORY_SIZE_STATELESS = 2; +const uint8_t HISTORY_SIZE_STATEFUL = 10; + +const uint8_t MAX_TYPENAME_LENGTH = 64; +const uint8_t MAX_TOPICNAME_LENGTH = 64; + +const int HEARTBEAT_STACKSIZE = 1200; // byte +const int THREAD_POOL_WRITER_STACKSIZE = 1100; // byte +const int THREAD_POOL_READER_STACKSIZE = 1600; // byte +const uint16_t SPDP_WRITER_STACKSIZE = 550; // byte + +const uint16_t SF_WRITER_HB_PERIOD_MS = 2000; +const uint16_t SPDP_RESEND_PERIOD_MS = 1000; +const uint8_t SPDP_CYCLECOUNT_HEARTBEAT = 2; // skip x SPDP rounds before checking liveliness +const uint8_t SPDP_WRITER_PRIO = 3; +const uint8_t SPDP_MAX_NUMBER_FOUND_PARTICIPANTS = 5; +const uint8_t SPDP_MAX_NUM_LOCATORS = 5; +const Duration_t SPDP_DEFAULT_REMOTE_LEASE_DURATION = { + 100, 0}; // Default lease duration for remote participants, usually + // overwritten by remote info +const Duration_t SPDP_MAX_REMOTE_LEASE_DURATION = { + 180, 0}; // Absolute maximum lease duration, ignoring remote participant info + +const int MAX_NUM_UDP_CONNECTIONS = 10; + +const int THREAD_POOL_NUM_WRITERS = 2; +const int THREAD_POOL_NUM_READERS = 2; +const int THREAD_POOL_WRITER_PRIO = 3; +const int THREAD_POOL_READER_PRIO = 3; +const int THREAD_POOL_WORKLOAD_QUEUE_LENGTH_USERTRAFFIC = 10; +const int THREAD_POOL_WORKLOAD_QUEUE_LENGTH_METATRAFFIC = 10; + +constexpr int OVERALL_HEAP_SIZE = THREAD_POOL_NUM_WRITERS * THREAD_POOL_WRITER_STACKSIZE + + THREAD_POOL_NUM_READERS * THREAD_POOL_READER_STACKSIZE + + MAX_NUM_PARTICIPANTS * SPDP_WRITER_STACKSIZE + + NUM_STATEFUL_WRITERS * HEARTBEAT_STACKSIZE; +} // namespace Config +} // namespace rtps + +#endif // RTPS_CONFIG_DESKTOP_H diff --git a/components/rtps_embedded/include/rtps/config_esp32.hpp b/components/rtps_embedded/include/rtps/config_esp32.hpp new file mode 100644 index 0000000000..53bfc65717 --- /dev/null +++ b/components/rtps_embedded/include/rtps/config_esp32.hpp @@ -0,0 +1,99 @@ +/* +The MIT License +Copyright (c) 2019 Lehrstuhl Informatik 11 - RWTH Aachen University +Modifications Copyright (c) 2026 ATDev +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: +The above copyright notice and this permission notice shall be included in +all copies or substantial portions of the Software. +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +THE SOFTWARE + +This file is part of embeddedRTPS. + +Author: i11 - Embedded Software, RWTH Aachen University +*/ + +#ifndef RTPS_CONFIG_ESP32_H +#define RTPS_CONFIG_ESP32_H + +#include "rtps/common/types.hpp" + +namespace rtps { + +#define IS_LITTLE_ENDIAN 1 +#define OS_IS_FREERTOS + +namespace Config { +const VendorId_t VENDOR_ID = {13, 37}; +const std::array IP_ADDRESS = {192, 168, 4, + 1}; // Fallback: must match DHCPS server netif IP. +const GuidPrefix_t BASE_GUID_PREFIX{1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 13}; + +const uint8_t DOMAIN_ID = 0; // 230 possible with UDP +const uint8_t NUM_STATELESS_WRITERS = 5; +const uint8_t NUM_STATELESS_READERS = 5; +const uint8_t NUM_STATEFUL_READERS = 5; +const uint8_t NUM_STATEFUL_WRITERS = 5; +const uint8_t MAX_NUM_PARTICIPANTS = 1; +const uint8_t NUM_WRITERS_PER_PARTICIPANT = 10; +const uint8_t NUM_READERS_PER_PARTICIPANT = 10; +const uint8_t NUM_WRITER_PROXIES_PER_READER = 6; +const uint8_t NUM_READER_PROXIES_PER_WRITER = 6; + +const uint8_t MAX_NUM_UNMATCHED_REMOTE_WRITERS = 50; +const uint8_t MAX_NUM_UNMATCHED_REMOTE_READERS = 50; + +const uint8_t MAX_NUM_READER_CALLBACKS = 5; + +const uint8_t HISTORY_SIZE_STATELESS = 2; +const uint8_t HISTORY_SIZE_STATEFUL = 10; + +const uint8_t MAX_TYPENAME_LENGTH = 64; +const uint8_t MAX_TOPICNAME_LENGTH = 64; + +const int HEARTBEAT_STACKSIZE = 1024 * 6; // byte +const int THREAD_POOL_WRITER_STACKSIZE = 4096; // byte +const int THREAD_POOL_READER_STACKSIZE = 1024 * 6; // byte +const uint16_t SPDP_WRITER_STACKSIZE = 4096; // byte + +const uint16_t SF_WRITER_HB_PERIOD_MS = 4000; +const uint16_t SPDP_RESEND_PERIOD_MS = 2000; +const uint8_t SPDP_CYCLECOUNT_HEARTBEAT = 2; // skip x SPDP rounds before checking liveliness +const uint8_t SPDP_WRITER_PRIO = 5; +const uint8_t SPDP_MAX_NUMBER_FOUND_PARTICIPANTS = 10; +const uint8_t SPDP_MAX_NUM_LOCATORS = 1; +const Duration_t SPDP_DEFAULT_REMOTE_LEASE_DURATION = { + 5, 0}; // Default lease duration for remote participants, usually + // overwritten by remote info +const Duration_t SPDP_MAX_REMOTE_LEASE_DURATION = { + 90, 0}; // Absolute maximum lease duration, ignoring remote participant info + +const Duration_t SPDP_LEASE_DURATION = {5, 0}; + +const int MAX_NUM_UDP_CONNECTIONS = 10; + +const int THREAD_POOL_NUM_WRITERS = 2; +const int THREAD_POOL_NUM_READERS = 2; +const int THREAD_POOL_WRITER_PRIO = 5; +const int THREAD_POOL_READER_PRIO = 5; +const int THREAD_POOL_WORKLOAD_QUEUE_LENGTH_USERTRAFFIC = 60; +const int THREAD_POOL_WORKLOAD_QUEUE_LENGTH_METATRAFFIC = 60; + +constexpr int OVERALL_HEAP_SIZE = THREAD_POOL_NUM_WRITERS * THREAD_POOL_WRITER_STACKSIZE + + THREAD_POOL_NUM_READERS * THREAD_POOL_READER_STACKSIZE + + MAX_NUM_PARTICIPANTS * SPDP_WRITER_STACKSIZE + + NUM_STATEFUL_WRITERS * HEARTBEAT_STACKSIZE; +} // namespace Config +} // namespace rtps + +#endif // RTPS_CONFIG_ESP32_H diff --git a/components/rtps_embedded/include/rtps/discovery/BuiltInEndpoints.hpp b/components/rtps_embedded/include/rtps/discovery/BuiltInEndpoints.hpp new file mode 100644 index 0000000000..dda5ada332 --- /dev/null +++ b/components/rtps_embedded/include/rtps/discovery/BuiltInEndpoints.hpp @@ -0,0 +1,46 @@ +/* +The MIT License +Copyright (c) 2019 Lehrstuhl Informatik 11 - RWTH Aachen University +Modifications Copyright (c) 2026 ATDev +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: +The above copyright notice and this permission notice shall be included in +all copies or substantial portions of the Software. +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +THE SOFTWARE + +This file is part of embeddedRTPS. + +Author: i11 - Embedded Software, RWTH Aachen University +*/ + +#ifndef RTPS_BUILTINENDPOINTS_H +#define RTPS_BUILTINENDPOINTS_H + +#include "rtps/entities/StatefulReader.hpp" +#include "rtps/entities/StatelessReader.hpp" +#include "rtps/entities/StatelessWriter.hpp" +#include "rtps/entities/Writer.hpp" + +namespace rtps { + +struct BuiltInEndpoints { + Writer *spdpWriter = nullptr; + Reader *spdpReader = nullptr; + Writer *sedpPubWriter = nullptr; + Reader *sedpPubReader = nullptr; + Writer *sedpSubWriter = nullptr; + Reader *sedpSubReader = nullptr; +}; +} // namespace rtps + +#endif // RTPS_BUILTINENDPOINTS_H diff --git a/components/rtps_embedded/include/rtps/discovery/ParticipantProxyData.hpp b/components/rtps_embedded/include/rtps/discovery/ParticipantProxyData.hpp new file mode 100644 index 0000000000..b9c18f8cd7 --- /dev/null +++ b/components/rtps_embedded/include/rtps/discovery/ParticipantProxyData.hpp @@ -0,0 +1,160 @@ +/* +The MIT License +Copyright (c) 2019 Lehrstuhl Informatik 11 - RWTH Aachen University +Modifications Copyright (c) 2026 ATDev +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: +The above copyright notice and this permission notice shall be included in +all copies or substantial portions of the Software. +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +THE SOFTWARE + +This file is part of embeddedRTPS. + +Author: i11 - Embedded Software, RWTH Aachen University +*/ + +#ifndef RTPS_PARTICIPANTPROXYDATA_H +#define RTPS_PARTICIPANTPROXYDATA_H + +#include "base_component.hpp" +#include "rtps/config.hpp" +#include "rtps/messages/MessageTypes.hpp" +#include "rtps/utils/CdrBuffer.hpp" +#include +#include +#include +#include +#include + +namespace rtps { + +class Participant; +using SMElement::ParameterId; + +using BuiltinEndpointSet_t = uint32_t; + +class ParticipantProxyData : public espp::BaseComponent { +public: + ParticipantProxyData() + : espp::BaseComponent("RtpsParticipantProxy", espp::Logger::Verbosity::WARN) { + onAliveSignal(); + } + explicit ParticipantProxyData(Guid_t guid); + + ProtocolVersion_t m_protocolVersion = PROTOCOLVERSION; + Guid_t m_guid = Guid_t{GUIDPREFIX_UNKNOWN, ENTITYID_UNKNOWN}; + VendorId_t m_vendorId = VENDOR_UNKNOWN; + bool m_expectsInlineQos = false; + BuiltinEndpointSet_t m_availableBuiltInEndpoints{0}; + std::array m_metatrafficUnicastLocatorList; + std::array m_metatrafficMulticastLocatorList; + std::array m_defaultUnicastLocatorList; + std::array m_defaultMulticastLocatorList; + Count_t m_manualLivelinessCount{1}; + Duration_t m_leaseDuration = Config::SPDP_DEFAULT_REMOTE_LEASE_DURATION; + std::chrono::time_point m_lastLivelinessReceivedTimestamp; + void reset(); + + /// Parses the SPDP PL_CDR parameter list starting at `buffer`'s current + /// position (the caller has already consumed the 4-byte encapsulation + /// header and configured the reader's endianness accordingly). + bool readFromBuffer(CdrReader &buffer, Participant *participant); + + inline bool hasParticipantWriter() const; + inline bool hasParticipantReader() const; + inline bool hasPublicationWriter() const; + inline bool hasPublicationReader() const; + inline bool hasSubscriptionWriter() const; + inline bool hasSubscriptionReader() const; + + inline void onAliveSignal(); + inline bool isAlive() const; + inline uint32_t getAliveSignalAgeInMilliseconds() const; + +private: + bool readLocatorIntoList(CdrReader &buffer, + std::array &list); + + static const BuiltinEndpointSet_t DISC_BUILTIN_ENDPOINT_PARTICIPANT_ANNOUNCER = 1 << 0; + static const BuiltinEndpointSet_t DISC_BUILTIN_ENDPOINT_PARTICIPANT_DETECTOR = 1 << 1; + static const BuiltinEndpointSet_t DISC_BUILTIN_ENDPOINT_PUBLICATION_ANNOUNCER = 1 << 2; + static const BuiltinEndpointSet_t DISC_BUILTIN_ENDPOINT_PUBLICATION_DETECTOR = 1 << 3; + static const BuiltinEndpointSet_t DISC_BUILTIN_ENDPOINT_SUBSCRIPTION_ANNOUNCER = 1 << 4; + static const BuiltinEndpointSet_t DISC_BUILTIN_ENDPOINT_SUBSCRIPTION_DETECTOR = 1 << 5; + static const BuiltinEndpointSet_t DISC_BUILTIN_ENDPOINT_PARTICIPANT_PROXY_ANNOUNCER = 1 << 6; + static const BuiltinEndpointSet_t DISC_BUILTIN_ENDPOINT_PARTICIPANT_PROXY_DETECTOR = 1 << 7; + static const BuiltinEndpointSet_t DISC_BUILTIN_ENDPOINT_PARTICIPANT_STATE_ANNOUNCER = 1 << 8; + static const BuiltinEndpointSet_t DISC_BUILTIN_ENDPOINT_PARTICIPANT_STATE_DETECTOR = 1 << 9; + static const BuiltinEndpointSet_t BUILTIN_ENDPOINT_PARTICIPANT_MESSAGE_DATA_WRITER = 1 << 10; + static const BuiltinEndpointSet_t BUILTIN_ENDPOINT_PARTICIPANT_MESSAGE_DATA_READER = 1 << 11; +}; + +// Needs to be in header because they are marked with inline +bool ParticipantProxyData::hasParticipantWriter() const { + return (m_availableBuiltInEndpoints & DISC_BUILTIN_ENDPOINT_PARTICIPANT_ANNOUNCER) == 1; +} + +bool ParticipantProxyData::hasParticipantReader() const { + return (m_availableBuiltInEndpoints & DISC_BUILTIN_ENDPOINT_PARTICIPANT_DETECTOR) != 0; +} + +bool ParticipantProxyData::hasPublicationWriter() const { + return (m_availableBuiltInEndpoints & DISC_BUILTIN_ENDPOINT_PUBLICATION_ANNOUNCER) != 0; +} + +bool ParticipantProxyData::hasPublicationReader() const { + return (m_availableBuiltInEndpoints & DISC_BUILTIN_ENDPOINT_PUBLICATION_DETECTOR) != 0; +} + +bool ParticipantProxyData::hasSubscriptionWriter() const { + return (m_availableBuiltInEndpoints & DISC_BUILTIN_ENDPOINT_SUBSCRIPTION_ANNOUNCER) != 0; +} + +bool ParticipantProxyData::hasSubscriptionReader() const { + return (m_availableBuiltInEndpoints & DISC_BUILTIN_ENDPOINT_SUBSCRIPTION_DETECTOR) != 0; +} + +void ParticipantProxyData::onAliveSignal() { + m_lastLivelinessReceivedTimestamp = std::chrono::steady_clock::now(); +} + +uint32_t ParticipantProxyData::getAliveSignalAgeInMilliseconds() const { + auto now = std::chrono::steady_clock::now(); + auto duration = std::chrono::duration_cast( + now - m_lastLivelinessReceivedTimestamp); + return static_cast(duration.count()); +} + +/* + * Returns true if last heartbeat within lease duration, else false + */ +bool ParticipantProxyData::isAlive() const { + const double factor = static_cast(1000) / + static_cast(1ULL << 32); // Convert fraction to milliseconds + uint32_t lease_in_ms = + m_leaseDuration.seconds * 1000 + static_cast(m_leaseDuration.fraction * factor); + + uint32_t max_lease_in_ms = + Config::SPDP_MAX_REMOTE_LEASE_DURATION.seconds * 1000 + + static_cast(Config::SPDP_MAX_REMOTE_LEASE_DURATION.fraction * factor); + + auto heatbeat_age_in_ms = getAliveSignalAgeInMilliseconds(); + + if (heatbeat_age_in_ms > std::min(lease_in_ms, max_lease_in_ms)) { + return false; + } + return true; +} + +} // namespace rtps +#endif // RTPS_PARTICIPANTPROXYDATA_H diff --git a/components/rtps_embedded/include/rtps/discovery/SEDPAgent.hpp b/components/rtps_embedded/include/rtps/discovery/SEDPAgent.hpp new file mode 100644 index 0000000000..ae60cec56b --- /dev/null +++ b/components/rtps_embedded/include/rtps/discovery/SEDPAgent.hpp @@ -0,0 +1,117 @@ +/* +The MIT License +Copyright (c) 2019 Lehrstuhl Informatik 11 - RWTH Aachen University +Modifications Copyright (c) 2026 ATDev +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: +The above copyright notice and this permission notice shall be included in +all copies or substantial portions of the Software. +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +THE SOFTWARE + +This file is part of embeddedRTPS. + +Author: i11 - Embedded Software, RWTH Aachen University +*/ + +#ifndef RTPS_SEDPAGENT_H +#define RTPS_SEDPAGENT_H + +#include "base_component.hpp" +#include "rtps/config.hpp" +#include "rtps/discovery/BuiltInEndpoints.hpp" +#include "rtps/discovery/TopicData.hpp" + +#include +#include + +namespace rtps { + +class Participant; +class ReaderCacheChange; +class Writer; +class Reader; + +class SEDPAgent : public espp::BaseComponent { +public: + SEDPAgent(); + void init(Participant &part, const BuiltInEndpoints &endpoints); + bool addWriter(Writer &writer); + bool addReader(Reader &reader); + bool deleteReader(Reader *reader); + bool deleteWriter(Writer *reader); + + void registerOnNewPublisherMatchedCallback(void (*callback)(void *arg), void *args); + void registerOnNewSubscriberMatchedCallback(void (*callback)(void *arg), void *args); + void removeUnmatchedEntitiesOfParticipant(const GuidPrefix_t &guidPrefix); + void removeUnmatchedEntity(const Guid_t &guid); + + uint32_t getNumRemoteUnmatchedReaders(); + uint32_t getNumRemoteUnmatchedWriters(); + +protected: // For testing purposes + void handlePublisherReaderMessage(const TopicData &writerData, const ReaderCacheChange &change); + void handleSubscriptionReaderMessage(const TopicData &writerData, + const ReaderCacheChange &change); + +private: + Participant *m_part = nullptr; + std::recursive_mutex m_mutex; + uint8_t m_buffer[600]; // TODO check size, currently changed from 300 to 600 + // (FastDDS gives too many options) + BuiltInEndpoints m_endpoints; + /* + * If we add readers later on, remote participants will not send matching + * writer proxies again (and vice versa). This is done only once during + * discovery. Therefore, we need to keep track of remote endpoints. Topic and + * type are represented as hash values to save memory. + */ + MemoryPool + m_unmatchedRemoteWriters; + size_t m_numUnmatchedRemoteWriters = 0; + MemoryPool + m_unmatchedRemoteReaders; + size_t m_numMatchedRemoteReaders = 0; + + void tryMatchUnmatchedEndpoints(); + void addUnmatchedRemoteWriter(const TopicData &writerData); + void addUnmatchedRemoteReader(const TopicData &readerData); + void addUnmatchedRemoteWriter(const TopicDataCompressed &writerData); + void addUnmatchedRemoteReader(const TopicDataCompressed &readerData); + + void handleRemoteEndpointDeletion(const TopicData &topic, const ReaderCacheChange &change); + + void (*mfp_onNewPublisherCallback)(void *arg) = nullptr; + void *m_onNewPublisherArgs = nullptr; + void (*mfp_onNewSubscriberCallback)(void *arg) = nullptr; + void *m_onNewSubscriberArgs = nullptr; + + static void jumppadPublisherReader(void *callee, const ReaderCacheChange &cacheChange); + static void jumppadSubscriptionReader(void *callee, const ReaderCacheChange &cacheChange); + + static void jumppadTakeProxyOfDisposedReader(const Reader *reader, const WriterProxy &proxy, + void *arg); + static void jumppadTakeProxyOfDisposedWriter(const Writer *writer, const ReaderProxy &proxy, + void *arg); + + void handlePublisherReaderMessage(const ReaderCacheChange &change); + void handleSubscriptionReaderMessage(const ReaderCacheChange &change); + + template bool deleteEndpoint(A *endpoint, Writer *sedp_endpoint); + + template bool announceEndpointDeletion(A *local_endpoint, Writer *sedp_endpoint); + + template bool disposeEndpointInSEDPHistory(A *local_endpoint, Writer *sedp_writer); +}; +} // namespace rtps + +#endif // RTPS_SEDPAGENT_H diff --git a/components/rtps_embedded/include/rtps/discovery/SPDPAgent.hpp b/components/rtps_embedded/include/rtps/discovery/SPDPAgent.hpp new file mode 100644 index 0000000000..616bcc409f --- /dev/null +++ b/components/rtps_embedded/include/rtps/discovery/SPDPAgent.hpp @@ -0,0 +1,96 @@ +/* +The MIT License +Copyright (c) 2019 Lehrstuhl Informatik 11 - RWTH Aachen University +Modifications Copyright (c) 2026 ATDev +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: +The above copyright notice and this permission notice shall be included in +all copies or substantial portions of the Software. +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +THE SOFTWARE + +This file is part of embeddedRTPS. + +Author: i11 - Embedded Software, RWTH Aachen University +*/ + +#ifndef RTPS_SPDP_H +#define RTPS_SPDP_H + +#include "base_component.hpp" +#include "rtps/common/types.hpp" +#include "rtps/config.hpp" +#include "rtps/discovery/BuiltInEndpoints.hpp" +#include "rtps/discovery/ParticipantProxyData.hpp" +#include "rtps/utils/CdrBuffer.hpp" +#include "rtps/utils/Log.hpp" +#include "task.hpp" + +#include +#include + +#if SPDP_VERBOSE && RTPS_GLOBAL_VERBOSE +#include "rtps/utils/printutils.hpp" +#define SPDP_LOG(...) logger_.warn(__VA_ARGS__) +#else +#define SPDP_LOG(...) \ + do { \ + } while (0) +#endif + +namespace rtps { +class Participant; +class Writer; +class Reader; +class ReaderCacheChange; + +class SPDPAgent : public espp::BaseComponent { +public: + SPDPAgent(); + void init(Participant &participant, BuiltInEndpoints &endpoints); + void start(); + void stop(); + /// True between start() and stop(); the Domain's protocol scheduler only + /// announces for running agents. + bool isRunning() const { return m_running; } + /// Send one SPDP announcement (+ periodic remote-liveliness check every + /// SPDP_CYCLECOUNT_HEARTBEAT rounds). Called by the Domain's protocol + /// scheduler at SPDP_RESEND_PERIOD_MS cadence instead of a dedicated + /// broadcast thread. + void announce(); + std::recursive_mutex m_mutex; + +private: + Participant *mp_participant = nullptr; + BuiltInEndpoints m_buildInEndpoints; + bool m_running = false; + std::array m_outputBuffer{}; // TODO check required size + std::array m_inputBuffer{}; + ParticipantProxyData m_proxyDataBuffer{}; + /// Number of valid bytes of the pre-built SPDP announcement in + /// m_outputBuffer (built once by addParticipantParameters()). + size_t m_outputSize = 0; + uint8_t m_cycleHB = 0; + + bool initialized = false; + static void receiveCallback(void *callee, const ReaderCacheChange &cacheChange); + void handleSPDPPackage(const ReaderCacheChange &cacheChange); + void processProxyData(); + bool addProxiesForBuiltInEndpoints(); + + void addInlineQos(CdrWriter &writer); + void addParticipantParameters(); + void endCurrentList(CdrWriter &writer); +}; +} // namespace rtps + +#endif // RTPS_SPDP_H diff --git a/components/rtps_embedded/include/rtps/discovery/TopicData.hpp b/components/rtps_embedded/include/rtps/discovery/TopicData.hpp new file mode 100644 index 0000000000..25fe57c5b6 --- /dev/null +++ b/components/rtps_embedded/include/rtps/discovery/TopicData.hpp @@ -0,0 +1,115 @@ +/* +The MIT License +Copyright (c) 2019 Lehrstuhl Informatik 11 - RWTH Aachen University +Modifications Copyright (c) 2026 ATDev +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: +The above copyright notice and this permission notice shall be included in +all copies or substantial portions of the Software. +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +THE SOFTWARE + +This file is part of embeddedRTPS. + +Author: i11 - Embedded Software, RWTH Aachen University +*/ + +#ifndef RTPS_DISCOVEREDWRITERDATA_H +#define RTPS_DISCOVEREDWRITERDATA_H + +#define SUPPRESS_UNICAST 0 + +#include "rtps/config.hpp" +#include "rtps/utils/CdrBuffer.hpp" +#include "rtps/utils/hash.hpp" +#include +#include +#include + +namespace rtps { + +struct BuiltInTopicKey { + std::array value; +}; + +struct TopicData { + Guid_t endpointGuid; + char typeName[Config::MAX_TYPENAME_LENGTH]; + char topicName[Config::MAX_TOPICNAME_LENGTH]; + ReliabilityKind_t reliabilityKind; + DurabilityKind_t durabilityKind; + FullLengthLocator unicastLocator; + FullLengthLocator multicastLocator; + + uint8_t statusInfo = 0; + bool statusInfoValid = false; + // Use Case: Remotes communicates id of deleted endpoint through key_hash + // parameter + EntityId_t entityIdFromKeyHash = ENTITYID_UNKNOWN; + bool entityIdFromKeyHashValid = false; + + TopicData() + : endpointGuid(GUID_UNKNOWN) + , typeName{'\0'} + , topicName{'\0'} + , reliabilityKind(ReliabilityKind_t::BEST_EFFORT) + , durabilityKind(DurabilityKind_t::VOLATILE) { + rtps::FullLengthLocator someLocator = + rtps::FullLengthLocator::createUDPv4Locator(192, 168, 0, 42, rtps::getUserUnicastPort(0)); + unicastLocator = someLocator; + multicastLocator = FullLengthLocator(); + }; + + TopicData(Guid_t guid, ReliabilityKind_t reliability, FullLengthLocator loc) + : endpointGuid(guid) + , typeName{'\0'} + , topicName{'\0'} + , reliabilityKind(reliability) + , durabilityKind(DurabilityKind_t::VOLATILE) + , unicastLocator(loc) {} + + bool matchesTopicOf(const TopicData &other); + + /// Parses the SEDP PL_CDR parameter list in `data` (little-endian; a + /// leading PL_CDR_LE encapsulation header, if present, is skipped as an + /// unknown zero-length parameter, matching the historical behavior). + bool readFromBuffer(std::span data); + /// Appends this endpoint's SEDP PL_CDR parameter list (including the + /// terminating PID_SENTINEL) to `writer`. + bool serializeInto(CdrWriter &writer) const; + + bool isDisposedFlagSet() const; + bool isUnregisteredFlagSet() const; +}; + +struct TopicDataCompressed { + Guid_t endpointGuid = GUID_UNKNOWN; + std::size_t topicHash = 0; + std::size_t typeHash = 0; + bool is_reliable = false; + LocatorIPv4 unicastLocator; + LocatorIPv4 multicastLocator; + + TopicDataCompressed() = default; + explicit TopicDataCompressed(const TopicData &topic_data) + : endpointGuid(topic_data.endpointGuid) + , topicHash(hashCharArray(topic_data.topicName, Config::MAX_TOPICNAME_LENGTH)) + , typeHash(hashCharArray(topic_data.typeName, Config::MAX_TYPENAME_LENGTH)) + , is_reliable(topic_data.reliabilityKind == ReliabilityKind_t::RELIABLE) + , unicastLocator(topic_data.unicastLocator) + , multicastLocator(topic_data.multicastLocator) {} + + bool matchesTopicOf(const TopicData &topic_data) const; +}; +} // namespace rtps + +#endif // RTPS_DISCOVEREDWRITERDATA_H diff --git a/components/rtps_embedded/include/rtps/entities/Domain.hpp b/components/rtps_embedded/include/rtps/entities/Domain.hpp new file mode 100644 index 0000000000..03e8d85605 --- /dev/null +++ b/components/rtps_embedded/include/rtps/entities/Domain.hpp @@ -0,0 +1,134 @@ +/* +The MIT License +Copyright (c) 2019 Lehrstuhl Informatik 11 - RWTH Aachen University +Modifications Copyright (c) 2026 ATDev +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: +The above copyright notice and this permission notice shall be included in +all copies or substantial portions of the Software. +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +THE SOFTWARE + +This file is part of embeddedRTPS. + +Author: i11 - Embedded Software, RWTH Aachen University +*/ + +#ifndef RTPS_DOMAIN_H +#define RTPS_DOMAIN_H + +#include "base_component.hpp" +#include "rtps/common/types.hpp" +#include "rtps/communication/EsppTransport.hpp" +#include "rtps/config.hpp" +#include "rtps/entities/Participant.hpp" +#include "rtps/entities/StatefulReader.hpp" +#include "rtps/entities/StatefulWriter.hpp" +#include "rtps/entities/StatelessReader.hpp" +#include "rtps/entities/StatelessWriter.hpp" +#include "task.hpp" +#include +#include +#include + +namespace rtps { +class Domain : public espp::BaseComponent { +public: + explicit Domain(const Ip4AddressBytes &localIpAddress); + Domain(EsppTransport &transport, const Ip4AddressBytes &localIpAddress); + ~Domain(); + + bool completeInit(); + void stop(); + + Participant *createParticipant(); + Writer *createWriter(Participant &part, const char *topicName, const char *typeName, + bool reliable, bool enforceUnicast = false); + Reader *createReader(Participant &part, const char *topicName, const char *typeName, + bool reliable, Ip4AddressBytes mcastaddress = {0, 0, 0, 0}); + + Writer *writerExists(Participant &part, const char *topicName, const char *typeName, + bool reliable); + Reader *readerExists(Participant &part, const char *topicName, const char *typeName, + bool reliable); + + bool deleteWriter(Participant &part, Writer *writer); + bool deleteReader(Participant &part, Reader *reader); + + void printInfo(); + +private: + friend class SizeInspector; + /// ReceiveCallback adapter: builds a PacketInfo from a raw datagram and + /// processes it inline on the transport worker that delivered it (the + /// reactor guarantees per-socket ordering). + static void datagramJumppad(void *arg, const uint8_t *data, std::size_t size, Ip4Port_t localPort, + Ip4Port_t remotePort, const Ip4AddressBytes &remoteAddress); + using DefaultTransport = EsppTransport; + DefaultTransport m_defaultTransport; + EsppTransport *m_transport = nullptr; + std::array m_participants; + Ip4AddressBytes m_localIpAddress{{0, 0, 0, 0}}; + const uint8_t PARTICIPANT_START_ID = 0; + /// Next participant id to try; ids may skip values when a unicast port is + /// already taken on this host (another process) and the id is probed forward. + ParticipantId_t m_nextParticipantId = PARTICIPANT_START_ID; + /// Number of participants created in this domain (slot count into + /// m_participants; independent of the probed participant ids). + uint8_t m_numParticipants = 0; + /// How many participant ids to probe for free unicast ports before giving up. + static constexpr uint8_t PARTICIPANT_PORT_PROBE_LIMIT = 16; + Participant *findParticipantById(ParticipantId_t id); + + /// Single deadline-scheduled protocol task: drives SPDP announcements for + /// every participant and heartbeat ticks for every stateful writer, + /// replacing one SPDP thread per participant plus one heartbeat thread per + /// stateful writer. Sleeps until the earliest deadline; publishes on + /// reliable writers nudge it awake (heartbeat piggyback). + bool protocolLoop(std::mutex &m, std::condition_variable &cv, bool ¬ified); + void nudgeProtocol(); + std::unique_ptr m_protocolTask; + std::mutex *m_protocolMutex = nullptr; + std::condition_variable *m_protocolCv = nullptr; + bool *m_protocolNotified = nullptr; + std::chrono::steady_clock::time_point m_nextSpdpAnnounce{}; + /// Set by stop() BEFORE the task is notified, so protocolLoop can tell a + /// stop apart from a heartbeat nudge (both arrive via the task cv). + std::atomic m_protocolStopRequested{false}; + + std::array m_statelessWriters; + std::array m_statelessReaders; + std::array m_statefulReaders; + std::array m_statefulWriters; + template B *getNextUnusedEndpoint(A &a) { + for (unsigned int i = 0; i < a.size(); i++) { + if (!a[i].isInitialized()) { + return &(a[i]); + } + } + return nullptr; + } + + bool m_initComplete = false; + bool m_transportSetupOk = true; + std::recursive_mutex m_mutex; + + void receiveCallback(const PacketInfo &packet); + GuidPrefix_t generateGuidPrefix(ParticipantId_t id) const; + void createBuiltinWritersAndReaders(Participant &part); + bool initializeTransport(); + void registerMulticastPort(FullLengthLocator mcastLocator); + static void receiveJumppad(void *callee, const PacketInfo &packet); +}; +} // namespace rtps + +#endif // RTPS_DOMAIN_H diff --git a/components/rtps_embedded/include/rtps/entities/Participant.hpp b/components/rtps_embedded/include/rtps/entities/Participant.hpp new file mode 100644 index 0000000000..bd5ddef3d0 --- /dev/null +++ b/components/rtps_embedded/include/rtps/entities/Participant.hpp @@ -0,0 +1,129 @@ +/* +The MIT License +Copyright (c) 2019 Lehrstuhl Informatik 11 - RWTH Aachen University +Modifications Copyright (c) 2026 ATDev +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: +The above copyright notice and this permission notice shall be included in +all copies or substantial portions of the Software. +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +THE SOFTWARE + +This file is part of embeddedRTPS. + +Author: i11 - Embedded Software, RWTH Aachen University +*/ + +#ifndef RTPS_PARTICIPANT_H +#define RTPS_PARTICIPANT_H + +#include "base_component.hpp" +#include "rtps/common/types.hpp" +#include "rtps/config.hpp" +#include "rtps/discovery/SEDPAgent.hpp" +#include "rtps/discovery/SPDPAgent.hpp" +#include "rtps/messages/MessageReceiver.hpp" + +#include +#include + +namespace rtps { + +class Writer; +class Reader; + +class Participant : public espp::BaseComponent { +public: + GuidPrefix_t m_guidPrefix; + ParticipantId_t m_participantId; + Ip4AddressBytes m_localIpAddress{{0, 0, 0, 0}}; + + Participant(); + explicit Participant(const GuidPrefix_t &guidPrefix, ParticipantId_t participantId); + + // Not allowed because the message receiver contains a pointer to the + // participant + Participant(const Participant &) = delete; + Participant(Participant &&) = delete; + Participant &operator=(const Participant &) = delete; + Participant &operator=(Participant &&) = delete; + + ~Participant(); + bool isValid(); + + void reuse(const GuidPrefix_t &guidPrefix, ParticipantId_t participantId); + void reuse(const GuidPrefix_t &guidPrefix, ParticipantId_t participantId, + const Ip4AddressBytes &localIpAddress); + + std::array getNextUserEntityKey(); + + // Actually the only two function that should be used by the user + bool registerOnNewPublisherMatchedCallback(void (*callback)(void *arg), void *args); + bool registerOnNewSubscriberMatchedCallback(void (*callback)(void *arg), void *args); + + //! Not-thread-safe function to add a writer + Writer *addWriter(Writer *writer); + bool isWritersFull(); + bool deleteWriter(Writer *writer); + + //! Not-thread-safe function to add a reader + Reader *addReader(Reader *reader); + bool isReadersFull(); + bool deleteReader(Reader *reader); + + //! (Probably) Thread safe if writers cannot be removed + Writer *getWriter(EntityId_t id); + Writer *getMatchingWriter(const TopicData &topicData); + Writer *getMatchingWriter(const TopicDataCompressed &topicData); + + //! (Probably) Thread safe if readers cannot be removed + Reader *getReader(EntityId_t id); + Reader *getReaderByWriterId(const Guid_t &guid); + Reader *getMatchingReader(const TopicData &topicData); + Reader *getMatchingReader(const TopicDataCompressed &topicData); + + bool addNewRemoteParticipant(const ParticipantProxyData &remotePart); + bool removeRemoteParticipant(const GuidPrefix_t &prefix); + void removeAllProxiesOfParticipant(const GuidPrefix_t &prefix); + void removeProxyFromAllEndpoints(const Guid_t &guid); + + const ParticipantProxyData *findRemoteParticipant(const GuidPrefix_t &prefix); + void refreshRemoteParticipantLiveliness(const GuidPrefix_t &prefix); + uint32_t getRemoteParticipantCount(); + MessageReceiver *getMessageReceiver(); + bool checkAndResetHeartbeats(); + + bool hasReaderWithMulticastLocator(const std::array &address); + + void addBuiltInEndpoints(BuiltInEndpoints &endpoints); + void newMessage(const uint8_t *data, DataSize_t size); + + SPDPAgent &getSPDPAgent(); + void printInfo(); + +private: + friend class SizeInspector; + MessageReceiver m_receiver; + bool m_hasBuilInEndpoints = false; + std::array m_nextUserEntityId{{0, 0, 1}}; + std::array m_writers = {nullptr}; + std::array m_readers = {nullptr}; + + std::recursive_mutex m_mutex; + MemoryPool m_remoteParticipants; + + SPDPAgent m_spdpAgent; + SEDPAgent m_sedpAgent; +}; +} // namespace rtps + +#endif // RTPS_PARTICIPANT_H diff --git a/components/rtps_embedded/include/rtps/entities/Reader.hpp b/components/rtps_embedded/include/rtps/entities/Reader.hpp new file mode 100644 index 0000000000..96e1558a87 --- /dev/null +++ b/components/rtps_embedded/include/rtps/entities/Reader.hpp @@ -0,0 +1,146 @@ +/* +The MIT License +Copyright (c) 2019 Lehrstuhl Informatik 11 - RWTH Aachen University +Modifications Copyright (c) 2026 ATDev +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: +The above copyright notice and this permission notice shall be included in +all copies or substantial portions of the Software. +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +THE SOFTWARE + +This file is part of embeddedRTPS. + +Author: i11 - Embedded Software, RWTH Aachen University +*/ + +#ifndef RTPS_READER_H +#define RTPS_READER_H + +#include "base_component.hpp" +#include "rtps/common/types.hpp" +#include "rtps/config.hpp" +#include "rtps/discovery/TopicData.hpp" +#include "rtps/entities/WriterProxy.hpp" +#include "rtps/storages/MemoryPool.hpp" +#include +#include + +namespace rtps { + +struct SubmessageHeartbeat; +struct SubmessageGap; + +class ReaderCacheChange { +private: + const uint8_t *data; + +public: + const ChangeKind_t kind; + const DataSize_t size; + const Guid_t writerGuid; + const SequenceNumber_t sn; + + ReaderCacheChange(ChangeKind_t kind, Guid_t &writerGuid, SequenceNumber_t sn, const uint8_t *data, + DataSize_t size) + : data(data) + , kind(kind) + , size(size) + , writerGuid(writerGuid) + , sn(sn){}; + + ~ReaderCacheChange() = default; // No need to free data. It's not owned by this object + // Not allowed because this class doesn't own the ptr and the user isn't + // allowed to use it outside the Scope of the callback + ReaderCacheChange(const ReaderCacheChange &other) = delete; + ReaderCacheChange(ReaderCacheChange &&other) = delete; + ReaderCacheChange &operator=(const ReaderCacheChange &other) = delete; + ReaderCacheChange &operator=(ReaderCacheChange &&other) = delete; + + bool copyInto(uint8_t *buffer, DataSize_t destSize) const { + if (destSize < size) { + return false; + } else { + memcpy(buffer, data, size); + return true; + } + } + + const uint8_t *getData() const { return data; } + + DataSize_t getDataSize() const { return size; } +}; + +typedef void (*ddsReaderCallback_fp)(void *callee, const ReaderCacheChange &cacheChange); + +class Reader : public espp::BaseComponent { +public: + using callbackFunction_t = void (*)(void *, const ReaderCacheChange &); + using callbackIdentifier_t = uint32_t; + + TopicData m_attributes; + virtual void newChange(const ReaderCacheChange &cacheChange) = 0; + virtual callbackIdentifier_t registerCallback(callbackFunction_t cb, void *arg); + virtual bool removeCallback(callbackIdentifier_t identifier); + uint8_t getNumCallbacks(); + + virtual bool onNewHeartbeat(const SubmessageHeartbeat &msg, const GuidPrefix_t &remotePrefix) = 0; + virtual bool onNewGapMessage(const SubmessageGap &msg, const GuidPrefix_t &remotePrefix) = 0; + virtual bool addNewMatchedWriter(const WriterProxy &newProxy) = 0; + virtual bool removeProxy(const Guid_t &guid); + virtual void removeAllProxiesOfParticipant(const GuidPrefix_t &guidPrefix); + bool isInitialized() const { return m_is_initialized_; } + virtual void reset(); + bool isProxy(const Guid_t &guid); + WriterProxy *getProxy(Guid_t guid); + uint32_t getProxiesCount(); + + void setSEDPSequenceNumber(const SequenceNumber_t &sn); + const SequenceNumber_t &getSEDPSequenceNumber(); + + using dumpProxyCallback = void (*)(const Reader *reader, const WriterProxy &, void *arg); + + int dumpAllProxies(dumpProxyCallback target, void *arg); + + virtual bool sendPreemptiveAckNack(const WriterProxy &writer); + +protected: + void executeCallbacks(const ReaderCacheChange &cacheChange); + bool initMutex(); + + SequenceNumber_t m_sedp_sequence_number; + + bool m_is_initialized_ = false; + Reader(); + virtual ~Reader() = default; + MemoryPool m_proxies; + + callbackIdentifier_t m_callback_identifier = 1; + + uint8_t m_callback_count = 0; + using callbackElement_t = struct { + callbackFunction_t function; + void *arg; + callbackIdentifier_t identifier; + }; + + std::array m_callbacks; + + // Guards manipulation of the proxies array + std::recursive_mutex m_proxies_mutex; + + // Guards manipulation of callback array + std::recursive_mutex m_callback_mutex; +}; +} // namespace rtps + +#endif // RTPS_READER_H diff --git a/components/rtps_embedded/include/rtps/entities/ReaderProxy.hpp b/components/rtps_embedded/include/rtps/entities/ReaderProxy.hpp new file mode 100644 index 0000000000..298f018f58 --- /dev/null +++ b/components/rtps_embedded/include/rtps/entities/ReaderProxy.hpp @@ -0,0 +1,68 @@ +/* +The MIT License +Copyright (c) 2019 Lehrstuhl Informatik 11 - RWTH Aachen University +Modifications Copyright (c) 2026 ATDev +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: +The above copyright notice and this permission notice shall be included in +all copies or substantial portions of the Software. +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +THE SOFTWARE + +This file is part of embeddedRTPS. + +Author: i11 - Embedded Software, RWTH Aachen University +*/ + +#ifndef RTPS_READERPROXY_H +#define RTPS_READERPROXY_H + +#include "rtps/common/types.hpp" +#include "rtps/discovery/ParticipantProxyData.hpp" + +namespace rtps { +struct ReaderProxy { + Guid_t remoteReaderGuid; + Count_t ackNackCount = {0}; + LocatorIPv4 remoteLocator; + bool is_reliable = false; + LocatorIPv4 remoteMulticastLocator; + bool useMulticast = false; + bool suppressUnicast = false; + bool unknown_eid = false; + bool finalFlag = false; + SequenceNumber_t lastAckNackSequenceNumber = {0, 1}; + + ReaderProxy() + : remoteReaderGuid({GUIDPREFIX_UNKNOWN, ENTITYID_UNKNOWN}) + , ackNackCount{0} + , remoteLocator(LocatorIPv4()) + , finalFlag(false){}; + ReaderProxy(const Guid_t &guid, const LocatorIPv4 &loc, bool reliable) + : remoteReaderGuid(guid) + , ackNackCount{0} + , remoteLocator(loc) + , is_reliable(reliable) + , finalFlag(false){}; + ReaderProxy(const Guid_t &guid, const LocatorIPv4 &loc, const LocatorIPv4 &mcastloc, + bool reliable) + : remoteReaderGuid(guid) + , ackNackCount{0} + , remoteLocator(loc) + , is_reliable(reliable) + , remoteMulticastLocator(mcastloc) + , finalFlag(false){}; +}; + +} // namespace rtps + +#endif // RTPS_READERPROXY_H diff --git a/components/rtps_embedded/include/rtps/entities/StatefulReader.hpp b/components/rtps_embedded/include/rtps/entities/StatefulReader.hpp new file mode 100644 index 0000000000..cef1c94791 --- /dev/null +++ b/components/rtps_embedded/include/rtps/entities/StatefulReader.hpp @@ -0,0 +1,65 @@ +/* +The MIT License +Copyright (c) 2019 Lehrstuhl Informatik 11 - RWTH Aachen University +Modifications Copyright (c) 2026 ATDev +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: +The above copyright notice and this permission notice shall be included in +all copies or substantial portions of the Software. +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +THE SOFTWARE + +This file is part of embeddedRTPS. + +Author: i11 - Embedded Software, RWTH Aachen University +*/ + +#ifndef RTPS_STATEFULREADER_H +#define RTPS_STATEFULREADER_H + +#include "rtps/common/types.hpp" +#include "rtps/communication/PacketInfo.hpp" +#include "rtps/config.hpp" +#include "rtps/entities/Reader.hpp" +#include "rtps/entities/WriterProxy.hpp" +#include "rtps/storages/MemoryPool.hpp" + +namespace rtps { +class EsppTransport; +struct SubmessageHeartbeat; + +template class StatefulReaderT final : public Reader { +public: + StatefulReaderT() + : m_srcPort(0) + , m_transport(nullptr) {} + ~StatefulReaderT() override; + bool init(const TopicData &attributes, NetworkDriver &driver); + void newChange(const ReaderCacheChange &cacheChange) override; + bool addNewMatchedWriter(const WriterProxy &newProxy) override; + bool onNewHeartbeat(const SubmessageHeartbeat &msg, const GuidPrefix_t &remotePrefix) override; + bool onNewGapMessage(const SubmessageGap &msg, const GuidPrefix_t &remotePrefix) override; + + bool sendPreemptiveAckNack(const WriterProxy &writer) override; + +private: + Ip4Port_t m_srcPort; // TODO intended for reuse but buffer not used as such + NetworkDriver *m_transport; +}; + +using StatefulReader = StatefulReaderT; + +} // namespace rtps + +#include "StatefulReader.tpp" + +#endif // RTPS_STATEFULREADER_H diff --git a/components/rtps_embedded/include/rtps/entities/StatefulReader.tpp b/components/rtps_embedded/include/rtps/entities/StatefulReader.tpp new file mode 100644 index 0000000000..96c5734199 --- /dev/null +++ b/components/rtps_embedded/include/rtps/entities/StatefulReader.tpp @@ -0,0 +1,282 @@ +/* +The MIT License +Copyright (c) 2019 Lehrstuhl Informatik 11 - RWTH Aachen University +Modifications Copyright (c) 2026 ATDev +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: +The above copyright notice and this permission notice shall be included in +all copies or substantial portions of the Software. +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +THE SOFTWARE + +This file is part of embeddedRTPS. + +Author: i11 - Embedded Software, RWTH Aachen University +*/ + +#include "rtps/entities/StatefulReader.hpp" +#include "rtps/messages/MessageFactory.hpp" +#include "rtps/storages/PayloadBuffer.hpp" +#include "rtps/utils/Diagnostics.hpp" +#include "rtps/utils/Log.hpp" +#include + +#if SFR_VERBOSE && RTPS_GLOBAL_VERBOSE +#include "rtps/utils/printutils.hpp" +#define SFR_LOG(...) logger_.warn(__VA_ARGS__) +#else +#define SFR_LOG(...) \ + do { \ + } while (0) +#endif + +using rtps::Guid_t; +using rtps::GuidPrefix_t; +using rtps::PacketInfo; +using rtps::ReaderCacheChange; +using rtps::SequenceNumber_t; +using rtps::SequenceNumberSet; +using rtps::StatefulReaderT; +using rtps::SubmessageGap; +using rtps::SubmessageHeartbeat; +using rtps::TopicData; +using rtps::WriterProxy; + +template StatefulReaderT::~StatefulReaderT() {} + +template +bool StatefulReaderT::init(const TopicData &attributes, NetworkDriver &driver) { + if (!initMutex()) { + return false; + } + + m_proxies.clear(); + m_attributes = attributes; + m_transport = &driver; + m_srcPort = attributes.unicastLocator.port; + m_is_initialized_ = true; + return true; +} + +template +void StatefulReaderT::newChange(const ReaderCacheChange &cacheChange) { + if (m_callback_count == 0 || !m_is_initialized_) { + return; + } + std::lock_guard lock(m_proxies_mutex); + for (auto &proxy : m_proxies) { + if (proxy.remoteWriterGuid == cacheChange.writerGuid) { + if (proxy.expectedSN == cacheChange.sn) { + SFR_LOG("Delivering SN {}.{} | GUID {} {} {} {}", (int)cacheChange.sn.high, + (int)cacheChange.sn.low, cacheChange.writerGuid.prefix.id[0], + cacheChange.writerGuid.prefix.id[1], cacheChange.writerGuid.prefix.id[2], + cacheChange.writerGuid.prefix.id[3]); + executeCallbacks(cacheChange); + ++proxy.expectedSN; + SFR_LOG("Done processing SN {}.{}", (int)cacheChange.sn.high, (int)cacheChange.sn.low); + return; + } else { + Diagnostics::StatefulReader::sfr_unexpected_sn++; + SFR_LOG("Unexpected SN {}.{} != {}.{}, dropping! GUID {} {} {} {}", + (int)proxy.expectedSN.high, (int)proxy.expectedSN.low, (int)cacheChange.sn.high, + (int)cacheChange.sn.low, cacheChange.writerGuid.prefix.id[0], + cacheChange.writerGuid.prefix.id[1], cacheChange.writerGuid.prefix.id[2], + cacheChange.writerGuid.prefix.id[3]); + } + } + } +} + +template +bool StatefulReaderT::addNewMatchedWriter(const WriterProxy &newProxy) { +#if SFR_VERBOSE && RTPS_GLOBAL_VERBOSE + SFR_LOG("New writer added"); +#endif + return m_proxies.add(newProxy); +} + +template +bool StatefulReaderT::onNewGapMessage(const SubmessageGap &msg, + const GuidPrefix_t &remotePrefix) { + std::lock_guard lock(m_proxies_mutex); + if (!m_is_initialized_) { + return false; + } + SFR_LOG("Processing gap message {}.{} {}.{}", (int)msg.gapStart.high, + (unsigned int)msg.gapStart.low, (int)msg.gapList.base.high, + (unsigned int)msg.gapList.base.low); + + Guid_t writerProxyGuid; + writerProxyGuid.prefix = remotePrefix; + writerProxyGuid.entityId = msg.writerId; + WriterProxy *writer = getProxy(writerProxyGuid); + + if (writer == nullptr) { + +#if SFR_VERBOSE && RTPS_GLOBAL_VERBOSE + SFR_LOG("Ignore GAP. Couldn't find a matching writer"); +#endif + return false; + } + + // Case 1: We are still waiting for messages before gapStart + if (writer->expectedSN < msg.gapStart) { + PacketInfo info; + info.srcPort = m_srcPort; + info.destAddr = writer->remoteLocator.getIp4AddressBytes(); + info.destPort = writer->remoteLocator.port; + PayloadBuffer payload; + rtps::MessageFactory::addHeader(payload, m_attributes.endpointGuid.prefix); + SequenceNumber_t last_valid = msg.gapStart; + --last_valid; + auto missing_sns = writer->getMissing(writer->expectedSN, last_valid); + rtps::MessageFactory::addAckNack(payload, msg.writerId, msg.readerId, missing_sns, + writer->getNextAckNackCount(), false); + info.payload = std::move(payload.bytes); + if (info.payload.empty()) { + return false; + } + m_transport->sendPacket(info); + return true; + } + + // Case 2: We are expecting a message between [gapStart; gapList.base -1] + // Advance expectedSN beyond gapList.base + if (writer->expectedSN < msg.gapList.base) { + writer->expectedSN = msg.gapList.base; + + // writer->expectedSN++; + + // Advance expectedSN to first unset bit + for (uint32_t bit = 0; bit < SNS_MAX_NUM_BITS; writer->expectedSN++, bit++) { + if (!msg.gapList.isSet(bit)) { + break; + } + } + + return true; + + } else { + + // Case 3: We are expecting a sequence number beyond gap list base, + // check if we need to update expectedSN + auto i = msg.gapList.base; + for (uint32_t bit = 0; bit < SNS_MAX_NUM_BITS; i++, bit++) { + if (i < writer->expectedSN) { + continue; + } + + if (msg.gapList.isSet(bit)) { + writer->expectedSN++; + } else { + PacketInfo info; + info.srcPort = m_srcPort; + info.destAddr = writer->remoteLocator.getIp4AddressBytes(); + info.destPort = writer->remoteLocator.port; + PayloadBuffer payload; + rtps::MessageFactory::addHeader(payload, m_attributes.endpointGuid.prefix); + SequenceNumberSet set; + set.base = writer->expectedSN; + set.numBits = 1; + set.bitMap[0] = set.bitMap[0] |= uint32_t{1} << 31; + rtps::MessageFactory::addAckNack(payload, msg.writerId, msg.readerId, set, + writer->getNextAckNackCount(), false); + info.payload = std::move(payload.bytes); + if (info.payload.empty()) { + return false; + } + m_transport->sendPacket(info); + + return true; + } + } + + return false; + } +} + +template +bool StatefulReaderT::onNewHeartbeat(const SubmessageHeartbeat &msg, + const GuidPrefix_t &sourceGuidPrefix) { + std::lock_guard lock(m_proxies_mutex); + if (!m_is_initialized_) { + return false; + } + PacketInfo info; + info.srcPort = m_srcPort; + PayloadBuffer payload; + + Guid_t writerProxyGuid; + writerProxyGuid.prefix = sourceGuidPrefix; + writerProxyGuid.entityId = msg.writerId; + WriterProxy *writer = getProxy(writerProxyGuid); + + if (writer == nullptr) { + +#if SFR_VERBOSE && RTPS_GLOBAL_VERBOSE + SFR_LOG("Ignore heartbeat. Couldn't find a matching writer"); +#endif + return false; + } + + if (writer->expectedSN < msg.firstSN) { + SFR_LOG("expectedSN < firstSN, advancing expectedSN"); + writer->expectedSN = msg.firstSN; + } + + writer->hbCount.value = msg.count.value; + info.destAddr = writer->remoteLocator.getIp4AddressBytes(); + info.destPort = writer->remoteLocator.port; + rtps::MessageFactory::addHeader(payload, m_attributes.endpointGuid.prefix); + auto missing_sns = writer->getMissing(msg.firstSN, msg.lastSN); + bool final_flag = (missing_sns.numBits == 0); + rtps::MessageFactory::addAckNack(payload, msg.writerId, msg.readerId, missing_sns, + writer->getNextAckNackCount(), final_flag); + + SFR_LOG("Sending acknack base {} bits {}.", (int)missing_sns.base.low, (int)missing_sns.numBits); + info.payload = std::move(payload.bytes); + if (info.payload.empty()) { + return false; + } + m_transport->sendPacket(info); + return true; +} + +template +bool StatefulReaderT::sendPreemptiveAckNack(const WriterProxy &writer) { + std::lock_guard lock(m_proxies_mutex); + if (!m_is_initialized_) { + return false; + } + + PacketInfo info; + info.srcPort = m_attributes.unicastLocator.port; + info.destAddr = writer.remoteLocator.getIp4AddressBytes(); + info.destPort = writer.remoteLocator.port; + PayloadBuffer payload; + rtps::MessageFactory::addHeader(payload, m_attributes.endpointGuid.prefix); + SequenceNumberSet number_set; + number_set.base.high = 0; + number_set.base.low = 0; + number_set.numBits = 0; + rtps::MessageFactory::addAckNack(payload, writer.remoteWriterGuid.entityId, + m_attributes.endpointGuid.entityId, number_set, Count_t{1}, + false); + + SFR_LOG("Sending preemptive acknack."); + info.payload = std::move(payload.bytes); + if (info.payload.empty()) { + return false; + } + m_transport->sendPacket(info); + return true; +} diff --git a/components/rtps_embedded/include/rtps/entities/StatefulWriter.hpp b/components/rtps_embedded/include/rtps/entities/StatefulWriter.hpp new file mode 100644 index 0000000000..c99be15863 --- /dev/null +++ b/components/rtps_embedded/include/rtps/entities/StatefulWriter.hpp @@ -0,0 +1,111 @@ +/* +The MIT License +Copyright (c) 2019 Lehrstuhl Informatik 11 - RWTH Aachen University +Modifications Copyright (c) 2026 ATDev +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: +The above copyright notice and this permission notice shall be included in +all copies or substantial portions of the Software. +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +THE SOFTWARE + +This file is part of embeddedRTPS. + +Author: i11 - Embedded Software, RWTH Aachen University +*/ + +#ifndef RTPS_STATEFULWRITER_H +#define RTPS_STATEFULWRITER_H + +#include "rtps/common/types.hpp" +#include "rtps/communication/PacketInfo.hpp" +#include "rtps/entities/ReaderProxy.hpp" +#include "rtps/entities/Writer.hpp" +#include "rtps/storages/HistoryCacheWithDeletion.hpp" +#include "rtps/storages/MemoryPool.hpp" +#include "rtps/storages/ThreadSafeCircularBuffer.hpp" +#include "task.hpp" + +#include + +namespace rtps { + +class EsppTransport; + +template class StatefulWriterT final : public Writer { +public: + StatefulWriterT() + : m_transport(nullptr) {} + ~StatefulWriterT() override; + bool init(TopicData attributes, TopicKind_t topicKind, NetworkDriver &driver, + bool enfUnicast = false); + + //! Executes required steps like sending packets. Intended to be called by + //! worker threads + void progress() override; + const CacheChange *newChange(ChangeKind_t kind, const uint8_t *data, DataSize_t size, + bool inLineQoS = false, + bool markDisposedAfterWrite = false) override; + + bool removeFromHistory(const SequenceNumber_t &s); + + /// Run one heartbeat evaluation, preserving the historical cadence: sends a + /// HEARTBEAT when due (period/4 while any proxy has unacknowledged changes, + /// full period otherwise), drops delayed dispose-after-write changes, and + /// returns the next time this writer wants to be ticked. Called by the + /// Domain's protocol scheduler task instead of a dedicated per-writer + /// heartbeat thread. + std::chrono::steady_clock::time_point heartbeatTick(std::chrono::steady_clock::time_point now); + + /// Install the scheduler nudge: invoked on newChange() so a publish + /// piggybacks an immediate heartbeat evaluation instead of waiting out the + /// current period. + void setProtocolNudge(std::function nudge) { m_protocolNudge = std::move(nudge); } + void setAllChangesToUnsent() override; + void onNewAckNack(const SubmessageAckNack &msg, const GuidPrefix_t &sourceGuidPrefix) override; + void reset() override; + void updateChangeKind(SequenceNumber_t &sequence_number); + +private: + NetworkDriver *m_transport = nullptr; + + HistoryCacheWithDeletion m_history; + + /* + * Cache changes marked as disposeAfterWrite are retained for a short amount + * in case of retransmission The whole 'disposeAfterWrite' mechanisms only + * exists to allow for repeated creation and deletion of endpoints during + * operation. Otherwise the history will quickly reach its limits. Will be + * replaced with something more elegant in the future. + */ + ThreadSafeCircularBuffer m_disposeWithDelay; + void dropDisposeAfterWriteChanges(); + + Count_t m_hbCount{1}; + + /// Next heartbeat deadline (managed by heartbeatTick / newChange). + std::chrono::steady_clock::time_point m_nextHeartbeat{}; + std::function m_protocolNudge{}; + + bool sendData(const ReaderProxy &reader, const CacheChange *next); + bool sendDataWRMulticast(const ReaderProxy &reader, const CacheChange *next); + void sendHeartBeat(); + void sendGap(const ReaderProxy &reader, const SequenceNumber_t &firstMissing, + const SequenceNumber_t &nextValid); +}; + +using StatefulWriter = StatefulWriterT; +} // namespace rtps + +#include "StatefulWriter.tpp" + +#endif // RTPS_STATEFULWRITER_H diff --git a/components/rtps_embedded/include/rtps/entities/StatefulWriter.tpp b/components/rtps_embedded/include/rtps/entities/StatefulWriter.tpp new file mode 100644 index 0000000000..45513cabc5 --- /dev/null +++ b/components/rtps_embedded/include/rtps/entities/StatefulWriter.tpp @@ -0,0 +1,551 @@ +/* +The MIT License +Copyright (c) 2019 Lehrstuhl Informatik 11 - RWTH Aachen University +Modifications Copyright (c) 2026 ATDev +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: +The above copyright notice and this permission notice shall be included in +all copies or substantial portions of the Software. +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +THE SOFTWARE + +This file is part of embeddedRTPS. + +Author: i11 - Embedded Software, RWTH Aachen University +*/ + +#include "rtps/entities/StatefulWriter.hpp" +#include "rtps/messages/MessageFactory.hpp" +#include "rtps/messages/MessageTypes.hpp" +#include "rtps/storages/PayloadBuffer.hpp" +#include "rtps/utils/Log.hpp" +#include +#include +#include +#include +#include +#include + +using rtps::CacheChange; +using rtps::GuidPrefix_t; +using rtps::ReaderProxy; +using rtps::SequenceNumber_t; +using rtps::StatefulWriterT; +using rtps::SubmessageAckNack; + +#if SFW_VERBOSE && RTPS_GLOBAL_VERBOSE +#include "rtps/utils/printutils.hpp" +#define SFW_LOG(...) logger_.warn(__VA_ARGS__) +#else +#define SFW_LOG(...) \ + do { \ + } while (0) +#endif + +template StatefulWriterT::~StatefulWriterT() = default; + +template +bool StatefulWriterT::init(TopicData attributes, TopicKind_t topicKind, + NetworkDriver &driver, + bool enfUnicast) { + + m_attributes = attributes; + + m_srcPort = attributes.unicastLocator.port; + m_enforceUnicast = enfUnicast; + m_topicKind = topicKind; + + m_nextSequenceNumberToSend = {0, 1}; + m_proxies.clear(); + + m_transport = &driver; + m_history.clear(); + m_hbCount = {1}; + + // Thread already exists, do not create new one (reusing slot case) + m_is_initialized_ = true; + + // Heartbeats are driven by the Domain's protocol scheduler via + // heartbeatTick(); no per-writer heartbeat thread. Arm the first evaluation. + m_nextHeartbeat = std::chrono::steady_clock::now(); + + return true; +} + +template void StatefulWriterT::reset() { + m_is_initialized_ = false; + // TODO +} + +template +const rtps::CacheChange * +StatefulWriterT::newChange(ChangeKind_t kind, const uint8_t *data, DataSize_t size, + bool inLineQoS, bool markDisposedAfterWrite) { + INIT_GUARD() + if (isIrrelevant(kind)) { + return nullptr; + } + + std::lock_guard lock(m_mutex); + if (!m_is_initialized_) { + return nullptr; + } + + if (m_history.isFull()) { + // Right now we drop elements anyway because we cannot detect non-responding + // readers yet. return nullptr; + SequenceNumber_t newMin = ++SequenceNumber_t(m_history.getCurrentSeqNumMin()); + if (m_nextSequenceNumberToSend < newMin) { + m_nextSequenceNumberToSend = newMin; // Make sure we have the correct sn to send + } + SFW_LOG("History full! Dropping changes {}.", this->m_attributes.topicName); + } + + auto *result = m_history.addChange(data, size, inLineQoS, markDisposedAfterWrite); + if (m_transport != nullptr) { + // Run the send asynchronously on the transport's worker pool (never inline + // under the caller's locks), matching the previous ThreadPool semantics. + m_transport->submit([this]() { progress(); }); + } + // Piggyback: pull the next heartbeat evaluation forward so a reliable + // publish is followed promptly by a HEARTBEAT instead of waiting out the + // period, and nudge the protocol scheduler. + m_nextHeartbeat = std::chrono::steady_clock::now(); + if (m_protocolNudge) { + m_protocolNudge(); + } + + SFW_LOG("Adding new data."); + + return result; +} + +template void StatefulWriterT::progress() { + INIT_GUARD() + std::lock_guard lock(m_mutex); + CacheChange *next = m_history.getChangeBySN(m_nextSequenceNumberToSend); + if (next != nullptr) { + uint32_t i = 0; + for (const auto &proxy : m_proxies) { + if (!m_enforceUnicast) { + sendDataWRMulticast(proxy, next); + } else { + i++; + sendData(proxy, next); + } + } + + SFW_LOG("Sending data with SN {}.{}", (int)m_nextSequenceNumberToSend.low, + (int)m_nextSequenceNumberToSend.high); + + /* + * Use case: deletion of local endpoints + * -> send Data Message with Disposed Flag set + * -> Set respective SEDP CacheChange as NOT_ALIVE_DISPOSED after + * transmission to proxies + * -> onAckNack will send Gap Messages to skip deleted local endpoints + * during SEDP + */ + if (next->disposeAfterWrite) { + SFW_LOG("Dispose after write msg sent to {} proxies", (int)i); + next->sentTime = std::chrono::steady_clock::now(); + if (!m_disposeWithDelay.copyElementIntoBuffer(next->sequenceNumber)) { + SFW_LOG("Failed to enqueue dispose after write!"); + m_history.dropChange(next->sequenceNumber); + } else { + SFW_LOG("Delayed dispose scheduled for sn {} {}", (int)next->sequenceNumber.high, + (int)next->sequenceNumber.low); + } + } + + ++m_nextSequenceNumberToSend; + SFW_LOG("HB from progress"); + sendHeartBeat(); + + } else { + SFW_LOG("Couldn't get a CacheChange with SN ({},{})", m_nextSequenceNumberToSend.high, + m_nextSequenceNumberToSend.low); + } +} + +template void StatefulWriterT::setAllChangesToUnsent() { + INIT_GUARD() + std::lock_guard lock(m_mutex); + + m_nextSequenceNumberToSend = m_history.getCurrentSeqNumMin(); + + if (m_transport != nullptr) { + // Run the send asynchronously on the transport's worker pool (never inline + // under the caller's locks), matching the previous ThreadPool semantics. + m_transport->submit([this]() { progress(); }); + } + // Piggyback: pull the next heartbeat evaluation forward so a reliable + // publish is followed promptly by a HEARTBEAT instead of waiting out the + // period, and nudge the protocol scheduler. + m_nextHeartbeat = std::chrono::steady_clock::now(); + if (m_protocolNudge) { + m_protocolNudge(); + } +} + +template +void StatefulWriterT::onNewAckNack(const SubmessageAckNack &msg, + const GuidPrefix_t &sourceGuidPrefix) { + INIT_GUARD() + std::lock_guard lock(m_mutex); + if (!m_is_initialized_) { + return; + } + + auto proxy_it = std::find_if(m_proxies.begin(), m_proxies.end(), [&](const auto &proxy) { + return proxy.remoteReaderGuid.prefix == sourceGuidPrefix && + proxy.remoteReaderGuid.entityId == msg.readerId; + }); + ReaderProxy *reader = (proxy_it != m_proxies.end()) ? &(*proxy_it) : nullptr; + + if (reader == nullptr) { +#if SFW_VERBOSE && RTPS_GLOBAL_VERBOSE + SFW_LOG("No proxy found with id: "); + printEntityId(msg.readerId); + SFW_LOG(" Dropping acknack.\n"); +#endif + return; + } + + reader->ackNackCount = msg.count; + reader->finalFlag = msg.header.finalFlag(); + reader->lastAckNackSequenceNumber = msg.readerSNState.base; + + rtps::SequenceNumber_t nextSN = msg.readerSNState.base; + + // Preemptive ack nack + if (nextSN.low == 0 && nextSN.high == 0) { + SFW_LOG("Received preemptive acknack, sending heartbeat."); + sendHeartBeat(); + return; + } + + if (m_history.isEmpty()) { + // We have never sent anything. Do not immediately respond with another + // heartbeat here, otherwise reader/writer can get stuck in HB<->ACKNACK + // ping-pong. Periodic heartbeat still handles liveliness. + if (m_history.getLastUsedSequenceNumber() == rtps::SequenceNumber_t{0, 0}) { + SFW_LOG("Ignoring acknack while history is empty and no samples were sent yet."); + return; + } else { + // No data but we have sent something in the past -> GapStart = + // readerSNState.base, NextValid = lastUsedSequenceNumber+1 + rtps::SequenceNumber_t nextValid = m_history.getLastUsedSequenceNumber(); + ++nextValid; + sendGap(*reader, msg.readerSNState.base, nextValid); + } + + return; + } + + // Requesting smaller SN than minimum sequence number -> sendGap + if (msg.readerSNState.base < m_history.getCurrentSeqNumMin()) { + sendGap(*reader, msg.readerSNState.base, m_history.getCurrentSeqNumMin()); + return; + } + + SFW_LOG("Received non-preemptive acknack with {} bits set.", msg.readerSNState.numBits); + for (uint32_t i = 0; + i < msg.readerSNState.numBits && nextSN <= m_history.getLastUsedSequenceNumber(); + ++i, ++nextSN) { + + if (msg.readerSNState.isSet(i)) { + + SFW_LOG("Looking for change {} | Bit {}", nextSN.low, i); + const rtps::CacheChange *cache = m_history.getChangeBySN(nextSN); + + // We still have the cache, send DATA + if (cache != nullptr) { + if (cache->disposeAfterWrite) { + SFW_LOG("Serving from dispose-after-write cache"); + } + sendData(*reader, cache); + } else { + SFW_LOG("> Change not found, search for next valid SN {}", nextSN.low); + // Cache not found, look for next valid SN + rtps::SequenceNumber_t gapBegin = nextSN; + rtps::CacheChange *nextValidChange = nullptr; + uint32_t j = i + 1; + for (++nextSN; nextSN <= m_history.getLastUsedSequenceNumber(); ++nextSN, ++j) { + nextValidChange = m_history.getChangeBySN(nextSN); + if (nextValidChange != nullptr) { + break; + } + } + if (nextValidChange == nullptr) { + sendGap(*reader, gapBegin, nextSN); + return; + } else { + sendGap(*reader, gapBegin, nextValidChange->sequenceNumber); + } + // sendData(nullptr, nextValidChange); + nextSN = nextValidChange->sequenceNumber; + --nextSN; + i = --j; + } + } + } +} + +template +bool rtps::StatefulWriterT::removeFromHistory(const SequenceNumber_t &s) { + std::lock_guard lock(m_mutex); + return m_history.dropChange(s); +} + +template +bool StatefulWriterT::sendData(const ReaderProxy &reader, const CacheChange *next) { + INIT_GUARD() + // TODO smarter packaging, e.g. create a message struct and serialize once. + + PacketInfo info; + info.srcPort = m_srcPort; + PayloadBuffer payload; + + MessageFactory::addHeader(payload, m_attributes.endpointGuid.prefix); + MessageFactory::addSubMessageTimeStamp(payload); + + // Just usable for IPv4 + const LocatorIPv4 &locator = reader.remoteLocator; + + info.destAddr = locator.getIp4AddressBytes(); + info.destPort = (Ip4Port_t)locator.port; + + MessageFactory::addSubMessageData(payload, next->data, next->inLineQoS, next->sequenceNumber, + m_attributes.endpointGuid.entityId, + reader.remoteReaderGuid.entityId); + info.payload = std::move(payload.bytes); + if (info.payload.empty()) { + return false; + } + m_transport->sendPacket(info); + + return true; +} + +template +void StatefulWriterT::sendGap(const ReaderProxy &reader, + const SequenceNumber_t &firstMissing, + const SequenceNumber_t &nextValid) { + INIT_GUARD() + // TODO smarter packaging, e.g. create a message struct and serialize once. + + PacketInfo info; + info.srcPort = m_srcPort; + PayloadBuffer payload; + + MessageFactory::addHeader(payload, m_attributes.endpointGuid.prefix); + MessageFactory::addSubMessageTimeStamp(payload); + + // Just usable for IPv4 + const LocatorIPv4 &locator = reader.remoteLocator; + + info.destAddr = locator.getIp4AddressBytes(); + info.destPort = (Ip4Port_t)locator.port; + + MessageFactory::addSubmessageGap(payload, m_attributes.endpointGuid.entityId, + reader.remoteReaderGuid.entityId, firstMissing, nextValid); + info.payload = std::move(payload.bytes); + if (info.payload.empty()) { + return; + } + m_transport->sendPacket(info); +} + +template +bool StatefulWriterT::sendDataWRMulticast(const ReaderProxy &reader, + const CacheChange *next) { + INIT_GUARD() + + if (reader.useMulticast || reader.suppressUnicast == false) { + PacketInfo info; + info.srcPort = m_srcPort; + PayloadBuffer payload; + + MessageFactory::addHeader(payload, m_attributes.endpointGuid.prefix); + MessageFactory::addSubMessageTimeStamp(payload); + + // Decide whether to use multicast or unicast. + if (reader.useMulticast) { + const LocatorIPv4 &locator = reader.remoteMulticastLocator; + info.destAddr = locator.getIp4AddressBytes(); + info.destPort = (Ip4Port_t)locator.port; + } else { + const LocatorIPv4 &locator = reader.remoteLocator; + info.destAddr = locator.getIp4AddressBytes(); + info.destPort = (Ip4Port_t)locator.port; + } + + EntityId_t reid; + if (reader.useMulticast) { + reid = ENTITYID_UNKNOWN; + } else { + reid = reader.remoteReaderGuid.entityId; + } + + MessageFactory::addSubMessageData(payload, next->data, next->inLineQoS, next->sequenceNumber, + m_attributes.endpointGuid.entityId, reid); + + info.payload = std::move(payload.bytes); + if (info.payload.empty()) { + return false; + } + m_transport->sendPacket(info); + } + return true; +} + +template +std::chrono::steady_clock::time_point +StatefulWriterT::heartbeatTick(std::chrono::steady_clock::time_point now) { + if (!m_is_initialized_) { + // Not ticking: report a far-future deadline so the scheduler ignores us. + return now + std::chrono::hours(24); + } + if (now < m_nextHeartbeat) { + return m_nextHeartbeat; + } + SFW_LOG("HB from tick"); + sendHeartBeat(); + dropDisposeAfterWriteChanges(); + const auto pending_ack_proxy = + std::find_if(m_proxies.begin(), m_proxies.end(), [&](const auto &proxy) { + return proxy.lastAckNackSequenceNumber < m_nextSequenceNumberToSend; + }); + const bool unconfirmed_changes = pending_ack_proxy != m_proxies.end(); + + // Same cadence as the historical per-writer loop: temporarily increase the + // HB frequency while there are unconfirmed remote changes. + if (unconfirmed_changes) { + SFW_LOG("HB speedup"); + m_nextHeartbeat = now + std::chrono::milliseconds(Config::SF_WRITER_HB_PERIOD_MS / 4); + } else { + m_nextHeartbeat = now + std::chrono::milliseconds(Config::SF_WRITER_HB_PERIOD_MS); + } + return m_nextHeartbeat; +} + +template void StatefulWriterT::dropDisposeAfterWriteChanges() { + SequenceNumber_t oldest_retained; + while (m_disposeWithDelay.peakFirst(oldest_retained)) { + + CacheChange *change = m_history.getChangeBySN(oldest_retained); + if (change == nullptr || !change->disposeAfterWrite) { + // Not in history anymore, drop + m_disposeWithDelay.moveFirstInto(oldest_retained); + return; + } + + if (change->sentTime == CacheChange::TimePoint{}) { + m_history.dropChange(change->sequenceNumber); + SequenceNumber_t tmp; + m_disposeWithDelay.moveFirstInto(tmp); + continue; + } + + auto age = std::chrono::steady_clock::now() - change->sentTime; + if (age > std::chrono::milliseconds(4000)) { + m_history.dropChange(change->sequenceNumber); + SFW_LOG("Removing SN {} {} for good", static_cast(oldest_retained.low), + static_cast(oldest_retained.high)); + SequenceNumber_t tmp; + m_disposeWithDelay.moveFirstInto(tmp); + + continue; + } else { + return; + } + } +} + +template void StatefulWriterT::sendHeartBeat() { + INIT_GUARD() + if (m_proxies.isEmpty() || !m_is_initialized_) { + + SFW_LOG("Skipping heartbeat. No proxies."); + return; + } + + for (auto &proxy : m_proxies) { + + PacketInfo info; + info.srcPort = m_srcPort; + PayloadBuffer payload; + + SequenceNumber_t firstSN; + SequenceNumber_t lastSN; + + MessageFactory::addHeader(payload, m_attributes.endpointGuid.prefix); + + { + std::lock_guard lock(m_mutex); + + if (!m_history.isEmpty()) { + firstSN = m_history.getCurrentSeqNumMin(); + lastSN = m_history.getCurrentSeqNumMax(); + + // Otherwise we may announce changes that have not been sent at least + // once! + if (lastSN > m_nextSequenceNumberToSend || lastSN == m_nextSequenceNumberToSend) { + lastSN = m_nextSequenceNumberToSend; + --lastSN; + } + + // Proxy has confirmed all sequence numbers and set final flag + if ((proxy.lastAckNackSequenceNumber > lastSN) && proxy.finalFlag && + proxy.ackNackCount.value > 0) { + SFW_LOG("Skipping heartbeat for proxy, all changes confirmed. lastSN {}.{}, lastAckNack " + "{}.{}", + (int)lastSN.low, (int)lastSN.high, (int)proxy.lastAckNackSequenceNumber.low, + (int)proxy.lastAckNackSequenceNumber.high); + continue; + } + } else if (m_history.getLastUsedSequenceNumber() == SequenceNumber_t{0, 0}) { + if ((proxy.lastAckNackSequenceNumber > m_history.getLastUsedSequenceNumber()) && + proxy.finalFlag && proxy.ackNackCount.value > 0) { + SFW_LOG("Skipping heartbeat for proxy, all changes confirmed. lastUsedSN {}.{}, " + "lastAckNack {}.{}", + (int)m_history.getLastUsedSequenceNumber().low, + (int)m_history.getLastUsedSequenceNumber().high, + (int)proxy.lastAckNackSequenceNumber.low, + (int)proxy.lastAckNackSequenceNumber.high); + continue; + } + firstSN = SequenceNumber_t{0, 1}; + lastSN = SequenceNumber_t{0, 0}; + } else { + firstSN = SequenceNumber_t{0, 1}; + lastSN = m_history.getLastUsedSequenceNumber(); + } + } + + SFW_LOG("Sending HB with SN range [{}.{};{}.{}]", firstSN.low, firstSN.high, lastSN.low, + lastSN.high); + + MessageFactory::addHeartbeat(payload, m_attributes.endpointGuid.entityId, + proxy.remoteReaderGuid.entityId, firstSN, lastSN, m_hbCount); + + info.destAddr = proxy.remoteLocator.getIp4AddressBytes(); + info.destPort = proxy.remoteLocator.port; + info.payload = std::move(payload.bytes); + if (info.payload.empty()) { + continue; + } + m_transport->sendPacket(info); + } + m_hbCount.value++; +} diff --git a/components/rtps_embedded/include/rtps/entities/StatelessReader.hpp b/components/rtps_embedded/include/rtps/entities/StatelessReader.hpp new file mode 100644 index 0000000000..4455f7c3b2 --- /dev/null +++ b/components/rtps_embedded/include/rtps/entities/StatelessReader.hpp @@ -0,0 +1,43 @@ +/* +The MIT License +Copyright (c) 2019 Lehrstuhl Informatik 11 - RWTH Aachen University +Modifications Copyright (c) 2026 ATDev +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: +The above copyright notice and this permission notice shall be included in +all copies or substantial portions of the Software. +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +THE SOFTWARE + +This file is part of embeddedRTPS. + +Author: i11 - Embedded Software, RWTH Aachen University +*/ + +#ifndef RTPS_STATELESSREADER_H +#define RTPS_STATELESSREADER_H + +#include "rtps/entities/Reader.hpp" + +namespace rtps { +class StatelessReader final : public Reader { +public: + bool init(const TopicData &attributes); + void newChange(const ReaderCacheChange &cacheChange) override; + bool onNewHeartbeat(const SubmessageHeartbeat &msg, const GuidPrefix_t &remotePrefix) override; + bool addNewMatchedWriter(const WriterProxy &newProxy) override; + bool onNewGapMessage(const SubmessageGap &msg, const GuidPrefix_t &remotePrefix) override; +}; + +} // namespace rtps + +#endif // RTPS_STATELESSREADER_H diff --git a/components/rtps_embedded/include/rtps/entities/StatelessWriter.hpp b/components/rtps_embedded/include/rtps/entities/StatelessWriter.hpp new file mode 100644 index 0000000000..abf86c567d --- /dev/null +++ b/components/rtps_embedded/include/rtps/entities/StatelessWriter.hpp @@ -0,0 +1,69 @@ +/* +The MIT License +Copyright (c) 2019 Lehrstuhl Informatik 11 - RWTH Aachen University +Modifications Copyright (c) 2026 ATDev +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: +The above copyright notice and this permission notice shall be included in +all copies or substantial portions of the Software. +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +THE SOFTWARE + +This file is part of embeddedRTPS. + +Author: i11 - Embedded Software, RWTH Aachen University +*/ + +#ifndef RTPS_RTPSWRITER_H +#define RTPS_RTPSWRITER_H + +#include "rtps/common/types.hpp" +#include "rtps/config.hpp" +#include "rtps/entities/Writer.hpp" +#include "rtps/storages/MemoryPool.hpp" +#include "rtps/storages/SimpleHistoryCache.hpp" + +namespace rtps { + +class EsppTransport; + +template class StatelessWriterT : public Writer { +public: + StatelessWriterT() + : m_transport(nullptr) {} + ~StatelessWriterT() override; + bool init(TopicData attributes, TopicKind_t topicKind, NetworkDriver &driver, + bool enfUnicast = false); + + void progress() override; + const CacheChange *newChange(ChangeKind_t kind, const uint8_t *data, DataSize_t size, + bool inLineQoS = false, + bool markDisposedAfterWrite = false) override; + bool removeFromHistory(const SequenceNumber_t &s); + + void setAllChangesToUnsent() override; + void onNewAckNack(const SubmessageAckNack &msg, const GuidPrefix_t &sourceGuidPrefix) override; + void reset() override; + +private: + NetworkDriver *m_transport; + + SimpleHistoryCache m_history; +}; + +using StatelessWriter = StatelessWriterT; + +} // namespace rtps + +#include "StatelessWriter.tpp" + +#endif // RTPS_RTPSWRITER_H diff --git a/components/rtps_embedded/include/rtps/entities/StatelessWriter.tpp b/components/rtps_embedded/include/rtps/entities/StatelessWriter.tpp new file mode 100644 index 0000000000..63eafc9b27 --- /dev/null +++ b/components/rtps_embedded/include/rtps/entities/StatelessWriter.tpp @@ -0,0 +1,212 @@ +/* +The MIT License +Copyright (c) 2019 Lehrstuhl Informatik 11 - RWTH Aachen University +Modifications Copyright (c) 2026 ATDev +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: +The above copyright notice and this permission notice shall be included in +all copies or substantial portions of the Software. +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +THE SOFTWARE + +This file is part of embeddedRTPS. + +Author: i11 - Embedded Software, RWTH Aachen University +*/ + +#include +#include + +#include "rtps/messages/MessageFactory.hpp" +#include "rtps/storages/PayloadBuffer.hpp" +#include "rtps/utils/Log.hpp" +#include "rtps/utils/udpUtils.hpp" +#include + +using rtps::CacheChange; +using rtps::GuidPrefix_t; +using rtps::SequenceNumber_t; +using rtps::StatelessWriterT; +using rtps::SubmessageAckNack; + +#if SLW_VERBOSE && RTPS_GLOBAL_VERBOSE +#include "rtps/utils/printutils.hpp" +#define SLW_LOG(...) logger_.warn(__VA_ARGS__) +#else +#define SLW_LOG(...) \ + do { \ + } while (0) +#endif + +template StatelessWriterT::~StatelessWriterT() { + // if(sys_mutex_valid(&m_mutex)){ + // sys_mutex_free(&m_mutex); + // } +} + +template +bool StatelessWriterT::init(TopicData attributes, TopicKind_t topicKind, + NetworkDriver &driver, + bool enfUnicast) { + + m_attributes = attributes; + + m_srcPort = attributes.unicastLocator.port; + m_enforceUnicast = enfUnicast; + + m_topicKind = topicKind; + m_nextSequenceNumberToSend = {0, 1}; + m_is_initialized_ = true; + + m_proxies.clear(); + m_history.clear(); + + m_transport = &driver; + + return true; +} + +template void StatelessWriterT::reset() { + m_is_initialized_ = false; +} + +template +const CacheChange *StatelessWriterT::newChange(rtps::ChangeKind_t kind, + const uint8_t *data, DataSize_t size, + bool inLineQoS, + bool markDisposedAfterWrite) { + INIT_GUARD(); + if (isIrrelevant(kind)) { + return nullptr; + } + std::lock_guard lock(m_mutex); + if (!m_is_initialized_) { + return nullptr; + } + + if (m_history.isFull()) { + SequenceNumber_t newMin = ++SequenceNumber_t(m_history.getSeqNumMin()); + if (m_nextSequenceNumberToSend < newMin) { + m_nextSequenceNumberToSend = newMin; // Make sure we have the correct sn to send + } + SLW_LOG("History is full, dropping oldest {}", this->m_attributes.topicName); + } + + auto *result = m_history.addChange(data, size); + if (m_transport != nullptr) { + // Run the send asynchronously on the transport's worker pool (never inline + // under the caller's locks), matching the previous ThreadPool semantics. + m_transport->submit([this]() { progress(); }); + } + + SLW_LOG("Adding new data."); + return result; +} + +template +bool StatelessWriterT::removeFromHistory(const SequenceNumber_t &s) { + return false; // Stateless Writers currently do not support deletion from + // history +} + +template void StatelessWriterT::setAllChangesToUnsent() { + INIT_GUARD(); + std::lock_guard lock(m_mutex); + + m_nextSequenceNumberToSend = m_history.getSeqNumMin(); + + if (m_transport != nullptr) { + // Run the send asynchronously on the transport's worker pool (never inline + // under the caller's locks), matching the previous ThreadPool semantics. + m_transport->submit([this]() { progress(); }); + } +} + +template +void StatelessWriterT::onNewAckNack(const SubmessageAckNack & /*msg*/, + const GuidPrefix_t &sourceGuidPrefix) { + INIT_GUARD(); + // Too lazy to respond +} + +template void StatelessWriterT::progress() { + INIT_GUARD(); + // TODO smarter packaging e.g. by creating MessageStruct and serializing + // after adjusting values. + + if (m_proxies.getNumElements() == 0) { + SLW_LOG("No proxy!"); + } + + for (const auto &proxy : m_proxies) { + + SLW_LOG("Progress."); + // Do nothing, if someone else sends for me... (Multicast) + if (proxy.useMulticast || !proxy.suppressUnicast || m_enforceUnicast) { + PacketInfo info; + info.srcPort = m_srcPort; + PayloadBuffer payload; + + MessageFactory::addHeader(payload, m_attributes.endpointGuid.prefix); + MessageFactory::addSubMessageTimeStamp(payload); + + { + std::lock_guard lock(m_mutex); + const CacheChange *next = m_history.getChangeBySN(m_nextSequenceNumberToSend); + if (next == nullptr) { + SLW_LOG("Couldn't get a new CacheChange with SN " + "(%li,%li)\n", + m_nextSequenceNumberToSend.high, m_nextSequenceNumberToSend.low); + return; + } else { + SLW_LOG("Sending change with SN ({},{})", m_nextSequenceNumberToSend.high, + m_nextSequenceNumberToSend.low); + } + + // Set EntityId to UNKNOWN if using multicast, because there might be + // different ones... + // TODO: mybe enhance by using UNKNOWN only if ids are really different + EntityId_t reid; + if (proxy.useMulticast && !m_enforceUnicast && proxy.unknown_eid) { + reid = ENTITYID_UNKNOWN; + } else { + reid = proxy.remoteReaderGuid.entityId; + } + MessageFactory::addSubMessageData(payload, next->data, false, next->sequenceNumber, + m_attributes.endpointGuid.entityId, + reid); // TODO + } + + info.payload = std::move(payload.bytes); + + // Just usable for IPv4 + // Decide which locator to be used unicast/multicast + + if (proxy.useMulticast && !m_enforceUnicast) { + info.destAddr = proxy.remoteMulticastLocator.getIp4AddressBytes(); + info.destPort = (Ip4Port_t)proxy.remoteMulticastLocator.port; + } else { + info.destAddr = proxy.remoteLocator.getIp4AddressBytes(); + info.destPort = (Ip4Port_t)proxy.remoteLocator.port; + } + SLW_LOG("Sending to {}.{}.{}.{}:{}", info.destAddr[0], info.destAddr[1], info.destAddr[2], + info.destAddr[3], info.destPort); + if (info.payload.empty()) { + continue; + } + m_transport->sendPacket(info); + } + } + + m_history.removeUntilIncl(m_nextSequenceNumberToSend); + ++m_nextSequenceNumberToSend; +} diff --git a/components/rtps_embedded/include/rtps/entities/Writer.hpp b/components/rtps_embedded/include/rtps/entities/Writer.hpp new file mode 100644 index 0000000000..eb3ba1c310 --- /dev/null +++ b/components/rtps_embedded/include/rtps/entities/Writer.hpp @@ -0,0 +1,113 @@ +/* +The MIT License +Copyright (c) 2019 Lehrstuhl Informatik 11 - RWTH Aachen University +Modifications Copyright (c) 2026 ATDev +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: +The above copyright notice and this permission notice shall be included in +all copies or substantial portions of the Software. +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +THE SOFTWARE + +This file is part of embeddedRTPS. + +Author: i11 - Embedded Software, RWTH Aachen University +*/ + +#ifndef RTPS_WRITER_H +#define RTPS_WRITER_H + +#include "base_component.hpp" +#include "rtps/discovery/TopicData.hpp" +#include "rtps/entities/ReaderProxy.hpp" +#include "rtps/storages/CacheChange.hpp" +#include "rtps/storages/MemoryPool.hpp" + +#include +#include + +#ifdef DEBUG_BUILD +#define COMPILE_INIT_GUARD +#endif + +#ifdef COMPILE_INIT_GUARD +#define INIT_GUARD() \ + if (!m_is_initialized_) { \ + logger_.error("Using uninitialized endpoint"); \ + while (1) { \ + } \ + } \ + } +#else +#define INIT_GUARD() // +#endif + +namespace rtps { + +class Writer : public espp::BaseComponent { +public: + TopicData m_attributes; + virtual bool addNewMatchedReader(const ReaderProxy &newProxy); + virtual bool removeProxy(const Guid_t &guid); + virtual void removeAllProxiesOfParticipant(const GuidPrefix_t &guidPrefix); + virtual void reset() = 0; + virtual const CacheChange *newChange(ChangeKind_t kind, const uint8_t *data, DataSize_t size); + + //! Executes required steps like sending packets. Intended to be called by + //! worker threads + virtual void progress() = 0; + + virtual bool removeFromHistory(const SequenceNumber_t &s) = 0; + virtual void setAllChangesToUnsent() = 0; + virtual void onNewAckNack(const SubmessageAckNack &msg, const GuidPrefix_t &sourceGuidPrefix) = 0; + + using dumpProxyCallback = void (*)(const Writer *writer, const ReaderProxy &, void *arg); + + int dumpAllProxies(dumpProxyCallback target, void *arg); + + bool isInitialized(); + std::uint32_t getProxiesCount(); + + void setSEDPSequenceNumber(const SequenceNumber_t &sn); + const SequenceNumber_t &getSEDPSequenceNumber(); + + bool isBuiltinEndpoint(); + +protected: + Writer(); + SequenceNumber_t m_sedp_sequence_number; + + std::recursive_mutex m_mutex; + + Ip4Port_t m_srcPort; + + bool m_enforceUnicast; + + TopicKind_t m_topicKind = TopicKind_t::NO_KEY; + SequenceNumber_t m_nextSequenceNumberToSend; + + friend class SEDPAgent; + virtual const CacheChange *newChange(ChangeKind_t kind, const uint8_t *data, DataSize_t size, + bool inLineQoS, bool markDisposedAfterWrite) = 0; + + friend class SizeInspector; + bool m_is_initialized_ = false; + virtual ~Writer() = default; + MemoryPool m_proxies; + + void resetSendOptions(); + void manageSendOptions(); + bool isIrrelevant(ChangeKind_t kind) const; +}; +} // namespace rtps + +#endif // RTPS_WRITER_H diff --git a/components/rtps_embedded/include/rtps/entities/WriterProxy.hpp b/components/rtps_embedded/include/rtps/entities/WriterProxy.hpp new file mode 100644 index 0000000000..d3f6f0edc7 --- /dev/null +++ b/components/rtps_embedded/include/rtps/entities/WriterProxy.hpp @@ -0,0 +1,80 @@ +/* +The MIT License +Copyright (c) 2019 Lehrstuhl Informatik 11 - RWTH Aachen University +Modifications Copyright (c) 2026 ATDev +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: +The above copyright notice and this permission notice shall be included in +all copies or substantial portions of the Software. +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +THE SOFTWARE + +This file is part of embeddedRTPS. + +Author: i11 - Embedded Software, RWTH Aachen University +*/ + +#ifndef RTPS_WRITERPROXY_H +#define RTPS_WRITERPROXY_H + +#include "rtps/common/types.hpp" +#include + +namespace rtps { +struct WriterProxy { + Guid_t remoteWriterGuid; + SequenceNumber_t expectedSN; + Count_t ackNackCount; + Count_t hbCount; + bool is_reliable = false; + LocatorIPv4 remoteLocator; + + WriterProxy() = default; + + WriterProxy(const Guid_t &guid, const LocatorIPv4 &loc, bool reliable) + : remoteWriterGuid(guid) + , expectedSN(SequenceNumber_t{0, 1}) + , ackNackCount{1} + , hbCount{0} + , is_reliable(reliable) + , remoteLocator(loc) {} + + // For now, we don't store any packets, so we just request all starting from + // the next expected + SequenceNumberSet getMissing(const SequenceNumber_t &firstAvail, + const SequenceNumber_t &lastAvail) const { + SequenceNumberSet set; + if (lastAvail < expectedSN) { + set.base = expectedSN; + set.numBits = 0; + } else { + set.base = expectedSN; + SequenceNumber_t i; + uint32_t bit; + for (bit = 0, i = expectedSN; i <= lastAvail && bit < SNS_MAX_NUM_BITS; i++, bit++) { + set.bitMap[0] |= uint32_t{1} << (31 - bit); + set.numBits++; + } + } + + return set; + } + + Count_t getNextAckNackCount() { + const Count_t tmp = ackNackCount; + ++ackNackCount.value; + return tmp; + } +}; +} // namespace rtps + +#endif // RTPS_WRITERPROXY_H diff --git a/components/rtps_embedded/include/rtps/messages/MessageFactory.hpp b/components/rtps_embedded/include/rtps/messages/MessageFactory.hpp new file mode 100644 index 0000000000..0507b6fec5 --- /dev/null +++ b/components/rtps_embedded/include/rtps/messages/MessageFactory.hpp @@ -0,0 +1,207 @@ +/* +The MIT License +Copyright (c) 2019 Lehrstuhl Informatik 11 - RWTH Aachen University +Modifications Copyright (c) 2026 ATDev +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: +The above copyright notice and this permission notice shall be included in +all copies or substantial portions of the Software. +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +THE SOFTWARE + +This file is part of embeddedRTPS. + +Author: i11 - Embedded Software, RWTH Aachen University +*/ + +// Copyright 2023 Apex.AI, Inc. +// All rights reserved. + +#ifndef RTPS_MESSAGEFACTORY_H +#define RTPS_MESSAGEFACTORY_H + +#include "rtps/common/types.hpp" +#include "rtps/config.hpp" +#include "rtps/messages/MessageTypes.hpp" +#include "rtps/utils/sysFunctions.hpp" + +#include +#include + +namespace rtps { +namespace MessageFactory { +const std::array PROTOCOL_TYPE{'R', 'T', 'P', 'S'}; +const uint8_t numBytesUntilEndOfLength = 4; // The first bytes incl. submessagelength don't count + +template void addHeader(Buffer &buffer, const GuidPrefix_t &guidPrefix) { + + Header header; + header.protocolName = PROTOCOL_TYPE; + header.protocolVersion = PROTOCOLVERSION; + header.vendorId = Config::VENDOR_ID; + header.guidPrefix = guidPrefix; + + serializeMessage(buffer, header); +} + +template bool addSubMessageInfoDST(Buffer &buffer, GuidPrefix_t &dst) { + SubmessageInfoDST msg; + msg.header.submessageId = SubmessageKind::INFO_DST; + +#if IS_LITTLE_ENDIAN + msg.header.flags = FLAG_LITTLE_ENDIAN; +#else + msg.header.flags = FLAG_BIG_ENDIAN; +#endif + + msg.header.octetsToNextHeader = sizeof(GuidPrefix_t); + msg.guidPrefix = dst; + + return serializeMessage(buffer, msg); +} + +template void addSubMessageTimeStamp(Buffer &buffer, bool setInvalid = false) { + SubmessageHeader header; + header.submessageId = SubmessageKind::INFO_TS; + +#if IS_LITTLE_ENDIAN + header.flags = FLAG_LITTLE_ENDIAN; +#else + header.flags = FLAG_BIG_ENDIAN; +#endif + + if (setInvalid) { + header.flags |= FLAG_INVALIDATE; + header.octetsToNextHeader = 0; + } else { + header.octetsToNextHeader = sizeof(Time_t); + } + + serializeMessage(buffer, header); + + if (!setInvalid) { + buffer.reserve(header.octetsToNextHeader); + Time_t now = getCurrentTimeStamp(); + buffer.append(reinterpret_cast(&now.seconds), sizeof(Time_t::seconds)); + buffer.append(reinterpret_cast(&now.fraction), sizeof(Time_t::fraction)); + } +} + +template +void addSubMessageData(Buffer &buffer, const PayloadBuffer &filledPayload, bool containsInlineQos, + const SequenceNumber_t &SN, const EntityId_t &writerID, + const EntityId_t &readerID) { + SubmessageData msg; + msg.header.submessageId = SubmessageKind::DATA; +#if IS_LITTLE_ENDIAN + msg.header.flags = FLAG_LITTLE_ENDIAN; +#else + msg.header.flags = FLAG_BIG_ENDIAN; +#endif + + msg.header.octetsToNextHeader = + SubmessageData::getRawSize() + filledPayload.spaceUsed() - numBytesUntilEndOfLength; + + if (containsInlineQos) { + msg.header.flags |= FLAG_INLINE_QOS; + } + if (filledPayload.isValid()) { + msg.header.flags |= FLAG_DATA_PAYLOAD; + } + + msg.writerSN = SN; + msg.extraFlags = 0; + msg.readerId = readerID; + msg.writerId = writerID; + + constexpr uint16_t octetsToInlineQoS = 4 + 4 + 8; // EntityIds + SequenceNumber + msg.octetsToInlineQos = octetsToInlineQoS; + + serializeMessage(buffer, msg); + + if (filledPayload.isValid()) { + buffer.append(filledPayload); + } +} + +template +void addHeartbeat(Buffer &buffer, EntityId_t writerId, EntityId_t readerId, + SequenceNumber_t firstSN, SequenceNumber_t lastSN, Count_t count) { + SubmessageHeartbeat subMsg; + subMsg.header.submessageId = SubmessageKind::HEARTBEAT; + subMsg.header.octetsToNextHeader = SubmessageHeartbeat::getRawSize() - numBytesUntilEndOfLength; +#if IS_LITTLE_ENDIAN + subMsg.header.flags = FLAG_LITTLE_ENDIAN; +#else + subMsg.header.flags = FLAG_BIG_ENDIAN; +#endif + // Force response by not setting final flag. + + subMsg.writerId = writerId; + subMsg.readerId = readerId; + subMsg.firstSN = firstSN; + subMsg.lastSN = lastSN; + subMsg.count = count; + + serializeMessage(buffer, subMsg); +} + +template +void addAckNack(Buffer &buffer, EntityId_t writerId, EntityId_t readerId, + SequenceNumberSet readerSNState, Count_t count, bool final_flag) { + SubmessageAckNack subMsg; + subMsg.header.submessageId = SubmessageKind::ACKNACK; +#if IS_LITTLE_ENDIAN + subMsg.header.flags = FLAG_LITTLE_ENDIAN; +#else + subMsg.header.flags = FLAG_BIG_ENDIAN; +#endif + if (final_flag) { + subMsg.header.flags |= FLAG_FINAL; // For now, we don't want any response + } else { + subMsg.header.flags &= ~FLAG_FINAL; // Send future heartbeats, even if no change occured + } + subMsg.header.octetsToNextHeader = + SubmessageAckNack::getRawSize(readerSNState) - numBytesUntilEndOfLength; + + subMsg.writerId = writerId; + subMsg.readerId = readerId; + subMsg.readerSNState = readerSNState; + subMsg.count = count; + + serializeMessage(buffer, subMsg); +} + +template +void addSubmessageGap(Buffer &buffer, EntityId_t writerId, EntityId_t readerId, + const SequenceNumber_t &firstMissing, const SequenceNumber_t &nextValid) { + SubmessageGap subMsg; + subMsg.header.submessageId = SubmessageKind::GAP; +#if IS_LITTLE_ENDIAN + subMsg.header.flags = FLAG_LITTLE_ENDIAN; +#else + subMsg.header.flags = FLAG_BIG_ENDIAN; +#endif + subMsg.header.octetsToNextHeader = 32; + + subMsg.writerId = writerId; + subMsg.readerId = readerId; + subMsg.gapStart = firstMissing; + subMsg.gapList.base = nextValid; + subMsg.gapList.numBits = 0; + + serializeMessage(buffer, subMsg); +} +} // namespace MessageFactory +} // namespace rtps + +#endif // RTPS_MESSAGEFACTORY_H diff --git a/components/rtps_embedded/include/rtps/messages/MessageReceiver.hpp b/components/rtps_embedded/include/rtps/messages/MessageReceiver.hpp new file mode 100644 index 0000000000..c0ec3e78dc --- /dev/null +++ b/components/rtps_embedded/include/rtps/messages/MessageReceiver.hpp @@ -0,0 +1,78 @@ +/* +The MIT License +Copyright (c) 2019 Lehrstuhl Informatik 11 - RWTH Aachen University +Modifications Copyright (c) 2026 ATDev +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: +The above copyright notice and this permission notice shall be included in +all copies or substantial portions of the Software. +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +THE SOFTWARE + +This file is part of embeddedRTPS. + +Author: i11 - Embedded Software, RWTH Aachen University +*/ + +#ifndef RTPS_MESSAGERECEIVER_H +#define RTPS_MESSAGERECEIVER_H + +#include "base_component.hpp" +#include "rtps/common/types.hpp" +#include "rtps/config.hpp" +#include "rtps/discovery/BuiltInEndpoints.hpp" +#include + +namespace rtps { +class Reader; +class Writer; +class Participant; +class MessageProcessingInfo; + +struct MessageSourceState { + GuidPrefix_t sourceGuidPrefix = GUIDPREFIX_UNKNOWN; + ProtocolVersion_t sourceVersion = PROTOCOLVERSION; + VendorId_t sourceVendor = VENDOR_UNKNOWN; + bool haveTimeStamp = false; +}; + +class MessageReceiver : public espp::BaseComponent { +public: + explicit MessageReceiver(Participant *part); + + bool processMessage(const uint8_t *data, DataSize_t size); + +private: + Participant *mp_part; + + // TODO make msgInfo a member + // This probably make processing faster, as no parameter needs to be passed + // around However, we need to make sure data is set to nullptr after + // processMsg to make sure we don't access it again afterwards. + /** + * Check header for validity, modifies the state of the receiver and + * adjusts the position of msgInfo accordingly + */ + bool processGapSubmessage(MessageProcessingInfo &msgInfo, const MessageSourceState &sourceState); + bool processHeader(MessageProcessingInfo &msgInfo, MessageSourceState &sourceState); + bool processSubmessage(MessageProcessingInfo &msgInfo, const SubmessageHeader &submsgHeader, + const MessageSourceState &sourceState); + bool processDataSubmessage(MessageProcessingInfo &msgInfo, const SubmessageHeader &submsgHeader, + const MessageSourceState &sourceState); + bool processHeartbeatSubmessage(MessageProcessingInfo &msgInfo, + const MessageSourceState &sourceState); + bool processAckNackSubmessage(MessageProcessingInfo &msgInfo, + const MessageSourceState &sourceState); +}; +} // namespace rtps + +#endif // RTPS_MESSAGERECEIVER_H diff --git a/components/rtps_embedded/include/rtps/messages/MessageTypes.hpp b/components/rtps_embedded/include/rtps/messages/MessageTypes.hpp new file mode 100644 index 0000000000..0f2a84367f --- /dev/null +++ b/components/rtps_embedded/include/rtps/messages/MessageTypes.hpp @@ -0,0 +1,415 @@ +/* +The MIT License +Copyright (c) 2019 Lehrstuhl Informatik 11 - RWTH Aachen University +Modifications Copyright (c) 2026 ATDev +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: +The above copyright notice and this permission notice shall be included in +all copies or substantial portions of the Software. +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +THE SOFTWARE + +This file is part of embeddedRTPS. + +Author: i11 - Embedded Software, RWTH Aachen University +*/ + +#ifndef RTPS_MESSAGES_H +#define RTPS_MESSAGES_H + +#include "rtps/common/types.hpp" + +#include +#include + +namespace rtps { + +#if defined(_MSC_VER) +#define RTPS_EMBEDDED_PACKED +#pragma pack(push, 1) +#else +#define RTPS_EMBEDDED_PACKED __attribute__((packed)) +#endif + +namespace SMElement { +// TODO endianess +enum ParameterId : uint16_t { + PID_PAD = 0x0000, + PID_SENTINEL = 0x0001, + PID_USER_DATA = 0x002c, + PID_TOPIC_NAME = 0x0005, + PID_TYPE_NAME = 0x0007, + PID_GROUP_DATA = 0x002d, + PID_TOPIC_DATA = 0x002e, + PID_DURABILITY = 0x001d, + PID_DURABILITY_SERVICE = 0x001e, + PID_DEADLINE = 0x0023, + PID_LATENCY_BUDGET = 0x0027, + PID_LIVELINESS = 0x001b, + PID_RELIABILITY = 0x001a, + PID_LIFESPAN = 0x002b, + PID_DESTINATION_ORDER = 0x0025, + PID_HISTORY = 0x0040, + PID_RESOURCE_LIMITS = 0x0041, + PID_OWNERSHIP = 0x001f, + PID_OWNERSHIP_STRENGTH = 0x0006, + PID_PRESENTATION = 0x0021, + PID_PARTITION = 0x0029, + PID_TIME_BASED_FILTER = 0x0004, + PID_TRANSPORT_PRIORITY = 0x0049, + PID_PROTOCOL_VERSION = 0x0015, + PID_VENDORID = 0x0016, + PID_UNICAST_LOCATOR = 0x002f, + PID_MULTICAST_LOCATOR = 0x0030, + PID_MULTICAST_IPADDRESS = 0x0011, + PID_DEFAULT_UNICAST_LOCATOR = 0x0031, + PID_DEFAULT_MULTICAST_LOCATOR = 0x0048, + PID_METATRAFFIC_UNICAST_LOCATOR = 0x0032, + PID_METATRAFFIC_MULTICAST_LOCATOR = 0x0033, + PID_DEFAULT_UNICAST_IPADDRESS = 0x000c, + PID_DEFAULT_UNICAST_PORT = 0x000e, + PID_METATRAFFIC_UNICAST_IPADDRESS = 0x0045, + PID_METATRAFFIC_UNICAST_PORT = 0x000d, + PID_METATRAFFIC_MULTICAST_IPADDRESS = 0x000b, + PID_METATRAFFIC_MULTICAST_PORT = 0x0046, + PID_EXPECTS_INLINE_QOS = 0x0043, + PID_PARTICIPANT_MANUAL_LIVELINESS_COUNT = 0x0034, + PID_PARTICIPANT_BUILTIN_ENDPOINTS = 0x0044, + PID_PARTICIPANT_LEASE_DURATION = 0x0002, + PID_CONTENT_FILTER_PROPERTY = 0x0035, + PID_PARTICIPANT_GUID = 0x0050, + PID_PARTICIPANT_ENTITYID = 0x0051, + PID_GROUP_GUID = 0x0052, + PID_GROUP_ENTITYID = 0x0053, + PID_BUILTIN_ENDPOINT_SET = 0x0058, + PID_PROPERTY_LIST = 0x0059, + PID_ENDPOINT_GUID = 0x005a, + PID_TYPE_MAX_SIZE_SERIALIZED = 0x0060, + PID_ENTITY_NAME = 0x0062, + PID_KEY_HASH = 0x0070, + PID_STATUS_INFO = 0x0071 +}; + +enum BuildInEndpointSet : uint32_t { + DISC_BIE_PARTICIPANT_ANNOUNCER = 1 << 0, + DISC_BIE_PARTICIPANT_DETECTOR = 1 << 1, + DISC_BIE_PUBLICATION_ANNOUNCER = 1 << 2, + DISC_BIE_PUBLICATION_DETECTOR = 1 << 3, + DISC_BIE_SUBSCRIPTION_ANNOUNCER = 1 << 4, + DISC_BIE_SUBSCRIPTION_DETECTOR = 1 << 5, + + DISC_BIE_PARTICIPANT_PROXY_ANNOUNCER = 1 << 6, + DISC_BIE_PARTICIPANT_PROXY_DETECTOR = 1 << 7, + DISC_BIE_PARTICIPANT_STATE_ANNOUNCER = 1 << 8, + DISC_BIE_PARTICIPANT_STATE_DETECTOR = 1 << 9, + + BIE_PARTICIPANT_MESSAGE_DATA_WRITER = 1 << 10, + BIE_PARTICIPANT_MESSAGE_DATA_READER = 1 << 11, +}; + +// TODO endianess + +const std::array SCHEME_CDR_LE{0x00, 0x01}; +const std::array SCHEME_PL_CDR_LE{0x00, 0x03}; + +struct ParameterList_t { + ParameterId pid; + uint16_t length; + // Values follow +} RTPS_EMBEDDED_PACKED; +} // namespace SMElement + +enum class SubmessageKind : uint8_t { + PAD = 0x01, /* Pad */ + ACKNACK = 0x06, /* AckNack */ + HEARTBEAT = 0x07, /* Heartbeat */ + GAP = 0x08, /* Gap */ + INFO_TS = 0x09, /* InfoTimestamp */ + INFO_SRC = 0x0c, /* InfoSource */ + INFO_REPLY_IP4 = 0x0d, /* InfoReplyIp4 */ + INFO_DST = 0x0e, /* InfoDestination */ + INFO_REPLY = 0x0f, /* InfoReply */ + NACK_FRAG = 0x12, /* NackFrag */ + HEARTBEAT_FRAG = 0x13, /* HeartbeatFrag */ + DATA = 0x15, /* Data */ + DATA_FRAG = 0x16 /* DataFrag */ +}; + +enum SubMessageFlag : uint8_t { + FLAG_ENDIANESS = (1 << 0), + FLAG_BIG_ENDIAN = 0, + FLAG_LITTLE_ENDIAN = (1 << 0), + FLAG_INVALIDATE = (1 << 1), + FLAG_INLINE_QOS = (1 << 1), + FLAG_NO_PAYLOAD = 0, + FLAG_DATA_PAYLOAD = (1 << 2), + FLAG_FINAL = (1 << 1), + FLAG_HB_LIVELINESS = (1 << 2) +}; + +const std::array RTPS_PROTOCOL_NAME = {'R', 'T', 'P', 'S'}; +struct Header { + std::array protocolName; + ProtocolVersion_t protocolVersion; + VendorId_t vendorId; + GuidPrefix_t guidPrefix; + static constexpr uint16_t getRawSize() { + return sizeof(std::array) + sizeof(ProtocolVersion_t) + sizeof(VendorId_t) + + sizeof(GuidPrefix_t); + } +}; + +struct SubmessageHeader { + SubmessageKind submessageId; + uint8_t flags; + uint16_t octetsToNextHeader; + static constexpr uint16_t getRawSize() { + return sizeof(SubmessageKind) + sizeof(uint8_t) + sizeof(uint16_t); + } + + bool finalFlag() const { return (flags & (SubMessageFlag::FLAG_FINAL)); } +}; + +struct SubmessageData { + SubmessageHeader header; + uint16_t extraFlags; + uint16_t octetsToInlineQos; + EntityId_t readerId; + EntityId_t writerId; + SequenceNumber_t writerSN; + static constexpr uint16_t getRawSize() { + return SubmessageHeader::getRawSize() + sizeof(uint16_t) + sizeof(uint16_t) + + (2 * 3 + 2 * 1) // EntityID + + sizeof(SequenceNumber_t); + } +}; + +struct SubmessageHeartbeat { + SubmessageHeader header; + EntityId_t readerId; + EntityId_t writerId; + SequenceNumber_t firstSN; + SequenceNumber_t lastSN; + Count_t count; + static constexpr uint16_t getRawSize() { + return SubmessageHeader::getRawSize() + (2 * 3 + 2 * 1) // EntityID + + 2 * sizeof(SequenceNumber_t) + sizeof(Count_t); + } +}; + +struct SubmessageInfoDST { + SubmessageHeader header; + GuidPrefix_t guidPrefix; + + static constexpr uint16_t getRawSize() { + return SubmessageHeader::getRawSize() + (sizeof(guidPrefix)); + } +}; + +struct SubmessageGap { + SubmessageHeader header; + EntityId_t readerId; + EntityId_t writerId; + SequenceNumber_t gapStart; + SequenceNumberSet gapList; + + static uint16_t getRawSizeWithoutSNSet() { + return SubmessageHeader::getRawSize() + (2 * (3 + 1) + 8); // 2*EntityID + GapStart + } + + static uint16_t getRawSizeWithSingleElementSNSet() { + return SubmessageHeader::getRawSize() + + (2 * (3 + 1) + 8 + 8 + 4); // 2*EntityID + GapStart + bitmapBase + numBits + } +}; + +struct SubmessageAckNack { + SubmessageHeader header; + EntityId_t readerId; + EntityId_t writerId; + SequenceNumberSet readerSNState; + Count_t count; + static uint16_t getRawSize(const SequenceNumberSet &set) { + uint16_t bitMapSize = 0; + if (set.numBits != 0) { + bitMapSize = static_cast(4 * (((set.numBits - 1) / 32) + 1)); + } + return getRawSizeWithoutSNSet() + sizeof(SequenceNumber_t) + sizeof(uint32_t) + + bitMapSize; // SequenceNumberSet + } + static uint16_t getRawSizeWithoutSNSet() { + return SubmessageHeader::getRawSize() + (2 * (3 + 1)) + sizeof(Count_t); + } +}; + +template bool serializeMessage(Buffer &buffer, Header &header) { + if (!buffer.reserve(Header::getRawSize())) { + return false; + } + + buffer.append(header.protocolName.data(), sizeof(std::array)); + buffer.append(reinterpret_cast(&header.protocolVersion), sizeof(ProtocolVersion_t)); + buffer.append(header.vendorId.vendorId.data(), sizeof(VendorId_t)); + buffer.append(header.guidPrefix.id.data(), sizeof(GuidPrefix_t)); + return true; +} + +template bool serializeMessage(Buffer &buffer, SubmessageHeader &header) { + if (!buffer.reserve(Header::getRawSize())) { + return false; + } + buffer.reserve(SubmessageHeader::getRawSize()); + + buffer.append(reinterpret_cast(&header.submessageId), sizeof(SubmessageKind)); + buffer.append(&header.flags, sizeof(uint8_t)); + buffer.append(reinterpret_cast(&header.octetsToNextHeader), sizeof(uint16_t)); + return true; +} + +template bool serializeMessage(Buffer &buffer, SubmessageInfoDST &msg) { + if (!buffer.reserve(SubmessageInfoDST::getRawSize())) { + return false; + } + + bool ret = serializeMessage(buffer, msg.header); + if (!ret) { + return false; + } + + return buffer.append(msg.guidPrefix.id.data(), sizeof(GuidPrefix_t)); +} + +template bool serializeMessage(Buffer &buffer, SubmessageData &msg) { + if (!buffer.reserve(SubmessageData::getRawSize())) { + return false; + } + + serializeMessage(buffer, msg.header); + + buffer.append(reinterpret_cast(&msg.extraFlags), sizeof(uint16_t)); + buffer.append(reinterpret_cast(&msg.octetsToInlineQos), sizeof(uint16_t)); + buffer.append(msg.readerId.entityKey.data(), msg.readerId.entityKey.size()); + buffer.append(reinterpret_cast(&msg.readerId.entityKind), sizeof(EntityKind_t)); + buffer.append(msg.writerId.entityKey.data(), msg.writerId.entityKey.size()); + buffer.append(reinterpret_cast(&msg.writerId.entityKind), sizeof(EntityKind_t)); + buffer.append(reinterpret_cast(&msg.writerSN.high), sizeof(msg.writerSN.high)); + buffer.append(reinterpret_cast(&msg.writerSN.low), sizeof(msg.writerSN.low)); + return true; +} + +template bool serializeMessage(Buffer &buffer, SubmessageHeartbeat &msg) { + if (!buffer.reserve(SubmessageHeartbeat::getRawSize())) { + return false; + } + + serializeMessage(buffer, msg.header); + + buffer.append(msg.readerId.entityKey.data(), msg.readerId.entityKey.size()); + buffer.append(reinterpret_cast(&msg.readerId.entityKind), sizeof(EntityKind_t)); + buffer.append(msg.writerId.entityKey.data(), msg.writerId.entityKey.size()); + buffer.append(reinterpret_cast(&msg.writerId.entityKind), sizeof(EntityKind_t)); + buffer.append(reinterpret_cast(&msg.firstSN.high), sizeof(msg.firstSN.high)); + buffer.append(reinterpret_cast(&msg.firstSN.low), sizeof(msg.firstSN.low)); + buffer.append(reinterpret_cast(&msg.lastSN.high), sizeof(msg.lastSN.high)); + buffer.append(reinterpret_cast(&msg.lastSN.low), sizeof(msg.lastSN.low)); + buffer.append(reinterpret_cast(&msg.count.value), sizeof(msg.count.value)); + return true; +} + +template bool serializeMessage(Buffer &buffer, SubmessageAckNack &msg) { + if (!buffer.reserve(SubmessageAckNack::getRawSize(msg.readerSNState))) { + return false; + } + + serializeMessage(buffer, msg.header); + + buffer.append(msg.readerId.entityKey.data(), msg.readerId.entityKey.size()); + buffer.append(reinterpret_cast(&msg.readerId.entityKind), sizeof(EntityKind_t)); + buffer.append(msg.writerId.entityKey.data(), msg.writerId.entityKey.size()); + buffer.append(reinterpret_cast(&msg.writerId.entityKind), sizeof(EntityKind_t)); + buffer.append(reinterpret_cast(&msg.readerSNState.base.high), + sizeof(msg.readerSNState.base.high)); + buffer.append(reinterpret_cast(&msg.readerSNState.base.low), + sizeof(msg.readerSNState.base.low)); + buffer.append(reinterpret_cast(&msg.readerSNState.numBits), sizeof(uint32_t)); + if (msg.readerSNState.numBits != 0) { + buffer.append(reinterpret_cast(msg.readerSNState.bitMap.data()), + 4 * (((msg.readerSNState.numBits - 1) / 32) + 1)); + } + buffer.append(reinterpret_cast(&msg.count.value), sizeof(msg.count.value)); + return true; +} + +template bool serializeMessage(Buffer &buffer, SubmessageGap &msg) { + if (msg.gapList.numBits != 0) { + return false; + } + if (!buffer.reserve(36)) { + return false; + } + + serializeMessage(buffer, msg.header); + + buffer.append(msg.readerId.entityKey.data(), msg.readerId.entityKey.size()); + buffer.append(reinterpret_cast(&msg.readerId.entityKind), sizeof(EntityKind_t)); + buffer.append(msg.writerId.entityKey.data(), msg.writerId.entityKey.size()); + buffer.append(reinterpret_cast(&msg.writerId.entityKind), sizeof(EntityKind_t)); + + buffer.append(reinterpret_cast(&msg.gapStart.high), sizeof(msg.gapStart.high)); + buffer.append(reinterpret_cast(&msg.gapStart.low), sizeof(msg.gapStart.low)); + + buffer.append(reinterpret_cast(&msg.gapList.base.high), sizeof(msg.gapList.base.high)); + buffer.append(reinterpret_cast(&msg.gapList.base.low), sizeof(msg.gapList.base.low)); + + buffer.append(reinterpret_cast(&msg.gapList.numBits), sizeof(uint32_t)); + + return true; +} + +struct MessageProcessingInfo { + MessageProcessingInfo(const uint8_t *data, DataSize_t size) + : data(data) + , size(size) {} + const uint8_t *data; + const DataSize_t size; + + //! Offset to the next unprocessed byte + DataSize_t nextPos = 0; + + inline const uint8_t *getPointerToCurrentPos() const { return &data[nextPos]; } + + //! Returns the size of data which isn't processed yet + inline DataSize_t getRemainingSize() const { return size - nextPos; } +}; + +bool deserializeMessage(const MessageProcessingInfo &info, Header &header); + +bool deserializeMessage(const MessageProcessingInfo &info, SubmessageHeader &header); + +bool deserializeMessage(const MessageProcessingInfo &info, SubmessageData &msg); + +bool deserializeMessage(const MessageProcessingInfo &info, SubmessageHeartbeat &msg); + +bool deserializeMessage(const MessageProcessingInfo &info, SubmessageAckNack &msg); + +bool deserializeMessage(const MessageProcessingInfo &info, SubmessageGap &msg); + +void deserializeSNS(const uint8_t *&position, SequenceNumberSet &set, std::size_t num_bitfields); + +} // namespace rtps + +#if defined(_MSC_VER) +#pragma pack(pop) +#endif +#undef RTPS_EMBEDDED_PACKED + +#endif // RTPS_MESSAGES_H diff --git a/components/rtps_embedded/include/rtps/rtps.hpp b/components/rtps_embedded/include/rtps/rtps.hpp new file mode 100644 index 0000000000..6838075b2f --- /dev/null +++ b/components/rtps_embedded/include/rtps/rtps.hpp @@ -0,0 +1,33 @@ +/* +The MIT License +Copyright (c) 2019 Lehrstuhl Informatik 11 - RWTH Aachen University +Modifications Copyright (c) 2026 ATDev +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: +The above copyright notice and this permission notice shall be included in +all copies or substantial portions of the Software. +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +THE SOFTWARE + +This file is part of embeddedRTPS. + +Author: i11 - Embedded Software, RWTH Aachen University +*/ + +#ifndef RTPS_RTPS_H +#define RTPS_RTPS_H + +#include "rtps/entities/Domain.hpp" + +namespace rtps {} // namespace rtps + +#endif // RTPS_RTPS_H diff --git a/components/rtps_embedded/include/rtps/storages/CacheChange.hpp b/components/rtps_embedded/include/rtps/storages/CacheChange.hpp new file mode 100644 index 0000000000..46bf8c64ea --- /dev/null +++ b/components/rtps_embedded/include/rtps/storages/CacheChange.hpp @@ -0,0 +1,74 @@ +/* +The MIT License +Copyright (c) 2019 Lehrstuhl Informatik 11 - RWTH Aachen University +Modifications Copyright (c) 2026 ATDev +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: +The above copyright notice and this permission notice shall be included in +all copies or substantial portions of the Software. +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +THE SOFTWARE + +This file is part of embeddedRTPS. + +Author: i11 - Embedded Software, RWTH Aachen University +*/ + +#ifndef PROJECT_CACHECHANGE_H +#define PROJECT_CACHECHANGE_H + +#include "rtps/common/types.hpp" +#include "rtps/storages/PayloadBuffer.hpp" + +#include + +namespace rtps { +struct CacheChange { + using TimePoint = std::chrono::steady_clock::time_point; + + ChangeKind_t kind = ChangeKind_t::INVALID; + bool inLineQoS = false; + bool disposeAfterWrite = false; + TimePoint sentTime{}; + SequenceNumber_t sequenceNumber = SEQUENCENUMBER_UNKNOWN; + PayloadBuffer data; + + CacheChange &operator=(const CacheChange &other) = delete; + + CacheChange &operator=(CacheChange &&other) noexcept { + kind = other.kind; + inLineQoS = other.inLineQoS; + disposeAfterWrite = other.disposeAfterWrite; + sentTime = other.sentTime; + sequenceNumber = other.sequenceNumber; + data = std::move(other.data); + return *this; + } + + CacheChange() = default; + CacheChange(ChangeKind_t kind, SequenceNumber_t sequenceNumber) + : kind(kind) + , sequenceNumber(sequenceNumber){}; + + void reset() { + kind = ChangeKind_t::INVALID; + sequenceNumber = SEQUENCENUMBER_UNKNOWN; + inLineQoS = false; + disposeAfterWrite = false; + sentTime = TimePoint{}; + } + + bool isInitialized() const { return (kind != ChangeKind_t::INVALID); } +}; +} // namespace rtps + +#endif // PROJECT_CACHECHANGE_H diff --git a/components/rtps_embedded/include/rtps/storages/HistoryCacheWithDeletion.hpp b/components/rtps_embedded/include/rtps/storages/HistoryCacheWithDeletion.hpp new file mode 100644 index 0000000000..7657a6b759 --- /dev/null +++ b/components/rtps_embedded/include/rtps/storages/HistoryCacheWithDeletion.hpp @@ -0,0 +1,276 @@ +/* +The MIT License +Copyright (c) 2019 Lehrstuhl Informatik 11 - RWTH Aachen University +Modifications Copyright (c) 2026 ATDev +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: +The above copyright notice and this permission notice shall be included in +all copies or substantial portions of the Software. +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +THE SOFTWARE + +This file is part of embeddedRTPS. + +Author: i11 - Embedded Software, RWTH Aachen University +*/ + +#ifndef HISTORYCACHEWITHDELETION_H +#define HISTORYCACHEWITHDELETION_H + +#include +#include + +namespace rtps { + +/** + * Extension of the SimpleHistoryCache that allows for deletion operation at the + * cost of efficieny + * TODO: Replace with something better in the future! + * Likely only used for SEDP + */ +template class HistoryCacheWithDeletion { +public: + HistoryCacheWithDeletion() = default; + + uint32_t m_dispose_after_write_cnt = 0; + + bool isFull() const { + uint16_t it = m_head; + incrementIterator(it); + return it == m_tail; + } + + const CacheChange *addChange(const uint8_t *data, DataSize_t size, bool inLineQoS, + bool disposeAfterWrite) { + CacheChange change; + change.kind = ChangeKind_t::ALIVE; + change.inLineQoS = inLineQoS; + change.disposeAfterWrite = disposeAfterWrite; + change.data.reserve(size); + change.data.append(data, size); + change.sequenceNumber = ++m_lastUsedSequenceNumber; + + if (disposeAfterWrite) { + m_dispose_after_write_cnt++; + } + + CacheChange *place = &m_buffer[m_head]; + incrementHead(); + + *place = std::move(change); + return place; + } + + const CacheChange *addChange(const uint8_t *data, DataSize_t size) { + return addChange(data, size, 0, false); + } + + void removeUntilIncl(SequenceNumber_t sn) { + if (m_head == m_tail) { + return; + } + + if (getCurrentSeqNumMax() <= sn) { // We won't overrun head + m_head = m_tail; + return; + } + + while (m_buffer[m_tail].sequenceNumber <= sn) { + incrementTail(); + } + } + + void dropOldest() { removeUntilIncl(getCurrentSeqNumMin()); } + + bool dropChange(const SequenceNumber_t &sn) { + uint16_t idx_to_clear; + CacheChange *change; + if (!getChangeBySN(sn, &change, idx_to_clear)) { + return false; // sn does not exist, nothing to do + } + + if (idx_to_clear == m_tail) { + m_buffer[m_tail].reset(); + incrementTail(); + return true; + } + + uint16_t prev = idx_to_clear; + do { + prev = idx_to_clear - 1; + if (prev >= m_buffer.size()) { + prev = m_buffer.size() - 1; + } + + m_buffer[idx_to_clear] = std::move(m_buffer[prev]); + idx_to_clear = prev; + + } while (prev != m_tail); + + incrementTail(); + + return true; + } + + bool setCacheChangeKind(const SequenceNumber_t &sn, ChangeKind_t kind) { + CacheChange *change = getChangeBySN(sn); + if (change == nullptr) { + return false; + } + + change->kind = kind; + return true; + } + + CacheChange *getChangeBySN(SequenceNumber_t sn) { + CacheChange *change; + uint16_t position; + if (getChangeBySN(sn, &change, position)) { + return change; + } else { + return nullptr; + } + } + + bool isEmpty() const { return (m_head == m_tail); } + + const SequenceNumber_t &getCurrentSeqNumMin() const { + if (m_head == m_tail) { + return SEQUENCENUMBER_UNKNOWN; + } else { + return m_buffer[m_tail].sequenceNumber; + } + } + + const SequenceNumber_t &getCurrentSeqNumMax() const { + if (m_head == m_tail) { + return SEQUENCENUMBER_UNKNOWN; + } else { + return m_lastUsedSequenceNumber; + } + } + + const SequenceNumber_t &getLastUsedSequenceNumber() const { return m_lastUsedSequenceNumber; } + + void clear() { + m_head = 0; + m_tail = 0; + m_lastUsedSequenceNumber = {0, 0}; + } +#ifdef DEBUG_HISTORY_CACHE_WITH_DELETION + void print() const { + for (unsigned int i = 0; i < m_buffer.size(); i++) { + std::cout << "[" << i << "] " + << " SN = " << m_buffer[i].sequenceNumber.low; + switch (m_buffer[i].kind) { + case ChangeKind_t::ALIVE: + std::cout << " Type = ALIVE"; + break; + case ChangeKind_t::INVALID: + std::cout << " Type = INVALID"; + break; + case ChangeKind_t::NOT_ALIVE_DISPOSED: + std::cout << " Type = DISPOSED"; + break; + } + if (m_head == i) { + std::cout << " <- HEAD"; + } + if (m_tail == i) { + std::cout << " <- TAIL"; + } + std::cout << std::endl; + } + } +#endif + bool isSNInRange(const SequenceNumber_t &sn) const { + if (isEmpty()) { + return false; + } + SequenceNumber_t minSN = getCurrentSeqNumMin(); + if (sn < minSN || getCurrentSeqNumMax() < sn) { + return false; + } + return true; + } + +private: + std::array m_buffer{}; + uint16_t m_head = 0; + uint16_t m_tail = 0; + static_assert(sizeof(SIZE) <= sizeof(m_head), "Iterator is large enough for given size"); + + SequenceNumber_t m_lastUsedSequenceNumber{0, 0}; + + bool getChangeBySN(const SequenceNumber_t &sn, CacheChange **out_change, + uint16_t &out_buffer_position) { + if (!isSNInRange(sn)) { + return false; + } + static_assert(std::is_unsigned::value, "Underflow well defined"); + static_assert(sizeof(m_tail) <= sizeof(uint16_t), "Cast ist well defined"); + + uint16_t cur_idx = m_tail; + while (cur_idx != m_head) { + if (m_buffer[cur_idx].sequenceNumber == sn) { + *out_change = &m_buffer[cur_idx]; + out_buffer_position = cur_idx; + return true; + } + // Sequence numbers are consecutive + if (m_buffer[cur_idx].sequenceNumber > sn) { + *out_change = nullptr; + return false; + } + + incrementIterator(cur_idx); + } + + *out_change = nullptr; + return false; + } + + inline void incrementHead() { + incrementIterator(m_head); + if (m_head == m_tail) { + // Move without check + incrementIterator(m_tail); // drop one + } + } + + inline void incrementIterator(uint16_t &iterator) const { + ++iterator; + if (iterator >= m_buffer.size()) { + iterator = 0; + } + } + + inline void incrementTail() { + if (m_buffer[m_tail].disposeAfterWrite) { + m_dispose_after_write_cnt--; + } + if (m_head != m_tail) { + m_buffer[m_tail].reset(); + incrementIterator(m_tail); + } + } + +protected: + // This constructor was created for unit testing + explicit HistoryCacheWithDeletion(SequenceNumber_t lastUsed) + : HistoryCacheWithDeletion() { + m_lastUsedSequenceNumber = lastUsed; + } +}; +} // namespace rtps + +#endif // HISTORYCACHEWITHDELETION_H diff --git a/components/rtps_embedded/include/rtps/storages/MemoryPool.hpp b/components/rtps_embedded/include/rtps/storages/MemoryPool.hpp new file mode 100644 index 0000000000..0bfa1e93ce --- /dev/null +++ b/components/rtps_embedded/include/rtps/storages/MemoryPool.hpp @@ -0,0 +1,186 @@ +/* +The MIT License +Copyright (c) 2019 Lehrstuhl Informatik 11 - RWTH Aachen University +Modifications Copyright (c) 2026 ATDev +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: +The above copyright notice and this permission notice shall be included in +all copies or substantial portions of the Software. +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +THE SOFTWARE + +This file is part of embeddedRTPS. + +Author: i11 - Embedded Software, RWTH Aachen University +*/ + +#ifndef RTPS_MEMORYPOOL_H +#define RTPS_MEMORYPOOL_H + +#include +#include +#include + +namespace rtps { + +template class MemoryPool { +public: + template class MemoryPoolIterator { + public: + using iterator_category = std::input_iterator_tag; + using value_type = IT_TYPE; + using difference_type = uint8_t; + using pointer = IT_TYPE *; + using reference = IT_TYPE &; + + explicit MemoryPoolIterator(MemoryPool &pool) + : m_pool(&pool) { + memcpy(m_bitMap, m_pool->m_bitMap, sizeof(m_bitMap)); + } + + bool operator==(const MemoryPoolIterator &other) const { return m_bit == other.m_bit; } + + bool operator!=(const MemoryPoolIterator &other) const { return !(*this == other); } + + reference operator*() const { return m_pool->m_data[m_bit]; } + + pointer operator->() const { return &m_pool->m_data[m_bit]; } + + // Pre-increment + MemoryPoolIterator &operator++() { + if (m_pool->m_numElements == 0) { + m_bit = SIZE; + return *this; + } + uint32_t bucket; + do { + ++m_bit; + bucket = m_bit / static_cast(8); + } while (!(m_bitMap[bucket] & (1 << (m_bit % 8))) && m_bit < SIZE); + + return *this; + } + + // Post-increment + MemoryPoolIterator operator++(int) { + MemoryPoolIterator tmp(*this); + ++(*this); + return tmp; + } + + private: + friend class MemoryPool; + MemoryPool *m_pool; + uint8_t m_bitMap[SIZE / 8 + 1]; + uint32_t m_bit = 0; + }; + + typedef MemoryPoolIterator MemPoolIter; + typedef MemoryPoolIterator const_MemPoolIter; + + typedef bool (*condition_fp)(TYPE); + + uint32_t getSize() { return SIZE; } + + bool isFull() const { return m_numElements == SIZE; } + + bool isEmpty() const { return m_numElements == 0; } + + uint32_t getNumElements() const { return m_numElements; } + + bool add(const TYPE &data) { + if (isFull()) { + printf("[MemoryPool] RESSOURCE LIMIT EXCEEDED \n"); + return false; + } + for (uint32_t bucket = 0; bucket < sizeof(m_bitMap); ++bucket) { + if (m_bitMap[bucket] != 0xFF) { + uint8_t byte = m_bitMap[bucket]; + for (uint8_t bit = 0; bit < 8; ++bit) { + if (!(byte & 1)) { + m_bitMap[bucket] |= 1 << bit; + m_data[bucket * 8 + bit] = data; + ++m_numElements; + return true; + } + byte = byte >> 1; + } + } + } + return false; + } + + /** + * Parameters are used in that way to allow lambdas with captures. Use this by + * creating two: E.g.: auto callback=[data](TYPE& value){return value == + * data;}; auto thunk=[](void* arg, TYPE& value){return + * (*static_cast(arg))(value);}; + * + * and then simply call: + * remove(thunk, &callback) + * + * NOTE: You have to make sure that the callback did not run out of scope. + */ + bool remove(bool (*jumppad)(void *, const TYPE &data), void *isCorrectElement) { + bool retcode = false; + for (auto it = begin(); it != end(); ++it) { + if (jumppad(isCorrectElement, *it)) { + const uint32_t bucket = it.m_bit / uint32_t{8}; + const uint32_t pos = + it.m_bit & uint32_t{7}; // 7 sets all bits above and including the one for 8 to 0 + m_bitMap[bucket] &= ~(static_cast(1) << pos); + --m_numElements; + retcode = true; + } + } + return retcode; + } + + void clear() { + for (unsigned int i = 0; i < (SIZE / 8 + 1); i++) { + m_bitMap[i] = 0; + } + m_numElements = 0; + } + + TYPE *find(bool (*jumppad)(void *, const TYPE &data), void *isCorrectElement) { + for (auto it = begin(); it != end(); ++it) { + if (jumppad(isCorrectElement, *it)) { + return &(*it); + } + } + return nullptr; + } + + MemPoolIter begin() { + MemPoolIter it(*this); + if (!(m_bitMap[0] & 1)) { + ++it; + } + return it; + } + + MemPoolIter end() { + MemPoolIter endIt(*this); + endIt.m_bit = SIZE; + return endIt; + } + +private: + uint8_t m_bitMap[SIZE / 8 + 1]{}; + uint32_t m_numElements = 0; + TYPE m_data[SIZE]; +}; + +} // namespace rtps + +#endif // RTPS_MEMORYPOOL_H diff --git a/components/rtps_embedded/include/rtps/storages/PayloadBuffer.hpp b/components/rtps_embedded/include/rtps/storages/PayloadBuffer.hpp new file mode 100644 index 0000000000..0637837dcb --- /dev/null +++ b/components/rtps_embedded/include/rtps/storages/PayloadBuffer.hpp @@ -0,0 +1,71 @@ +/* +The MIT License +Copyright (c) 2019 Lehrstuhl Informatik 11 - RWTH Aachen University +Modifications Copyright (c) 2026 ATDev +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: +The above copyright notice and this permission notice shall be included in +all copies or substantial portions of the Software. +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +THE SOFTWARE + +This file is part of embeddedRTPS. + +Author: i11 - Embedded Software, RWTH Aachen University +*/ + +#ifndef RTPS_PAYLOADBUFFER_H +#define RTPS_PAYLOADBUFFER_H + +#include + +#include "rtps/common/types.hpp" + +namespace rtps { + +struct PayloadBuffer { + std::vector bytes; + + bool isValid() const { return true; } + + bool reserve(DataSize_t length) { + if (length > bytes.max_size() - bytes.size()) { + return false; + } + bytes.reserve(bytes.size() + length); + return true; + } + + bool append(const uint8_t *data, DataSize_t length) { + if (length == 0) { + return true; + } + if (data == nullptr) { + return false; + } + + bytes.insert(bytes.end(), data, data + length); + return true; + } + + void append(const PayloadBuffer &other) { + bytes.insert(bytes.end(), other.bytes.begin(), other.bytes.end()); + } + + DataSize_t spaceUsed() const { return static_cast(bytes.size()); } + + void reset() { bytes.clear(); } +}; + +} // namespace rtps + +#endif // RTPS_PAYLOADBUFFER_H diff --git a/components/rtps_embedded/include/rtps/storages/SimpleHistoryCache.hpp b/components/rtps_embedded/include/rtps/storages/SimpleHistoryCache.hpp new file mode 100644 index 0000000000..540560dbca --- /dev/null +++ b/components/rtps_embedded/include/rtps/storages/SimpleHistoryCache.hpp @@ -0,0 +1,177 @@ +/* +The MIT License +Copyright (c) 2019 Lehrstuhl Informatik 11 - RWTH Aachen University +Modifications Copyright (c) 2026 ATDev +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: +The above copyright notice and this permission notice shall be included in +all copies or substantial portions of the Software. +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +THE SOFTWARE + +This file is part of embeddedRTPS. + +Author: i11 - Embedded Software, RWTH Aachen University +*/ + +#ifndef PROJECT_SIMPLEHISTORYCACHE_H +#define PROJECT_SIMPLEHISTORYCACHE_H + +#include "rtps/config.hpp" +#include "rtps/storages/CacheChange.hpp" + +namespace rtps { + +/** + * Simple version of a history cache. It sets consecutive sequence numbers + * automatically which allows an easy and fast approach of dropping acknowledged + * changes. Furthermore, disposing of arbitrary changes is not possible. + * However, this is in principle easy to add by changing the ChangeKind and + * dropping it when passing it during deleting of other sequence numbers + */ +template class SimpleHistoryCache { +public: + SimpleHistoryCache() = default; + + bool isFull() const { + uint16_t it = m_head; + incrementIterator(it); + return it == m_tail; + } + + const CacheChange *addChange(const uint8_t *data, DataSize_t size, bool inLineQoS, + bool disposeAfterWrite) { + CacheChange change; + change.kind = ChangeKind_t::ALIVE; + change.inLineQoS = inLineQoS; + change.disposeAfterWrite = disposeAfterWrite; + change.data.reserve(size); + change.data.append(data, size); + change.sequenceNumber = ++m_lastUsedSequenceNumber; + + CacheChange *place = &m_buffer[m_head]; + incrementHead(); + + *place = std::move(change); + return place; + } + + const CacheChange *addChange(const uint8_t *data, DataSize_t size) { + return addChange(data, size, 0, false); + } + + void removeUntilIncl(SequenceNumber_t sn) { + if (m_head == m_tail) { + return; + } + + if (getSeqNumMax() <= sn) { // We won't overrun head + m_head = m_tail; + return; + } + + while (m_buffer[m_tail].sequenceNumber <= sn && (m_head != m_tail)) { + incrementTail(); + } + } + + void dropOldest() { removeUntilIncl(getSeqNumMin()); } + + bool setCacheChangeKind(const SequenceNumber_t &sn, ChangeKind_t kind) { + CacheChange *change = getChangeBySN(sn); + if (change == nullptr) { + return false; + } + + change->kind = kind; + return true; + } + + CacheChange *getChangeBySN(SequenceNumber_t sn) { + SequenceNumber_t minSN = getSeqNumMin(); + if (sn < minSN || getSeqNumMax() < sn) { + return nullptr; + } + static_assert(std::is_unsigned::value, "Underflow well defined"); + static_assert(sizeof(m_tail) <= sizeof(uint16_t), "Cast ist well defined"); + // We don't overtake head, therefore difference of sn is within same range + // as iterators + uint16_t pos = m_tail + static_cast(sn.low - minSN.low); + + // Diff is smaller than the size of the array -> max one overflow + if (pos >= m_buffer.size()) { + pos -= m_buffer.size(); + } + return &m_buffer[pos]; + } + + const SequenceNumber_t &getSeqNumMin() const { + if (m_head == m_tail) { + return SEQUENCENUMBER_UNKNOWN; + } else { + return m_buffer[m_tail].sequenceNumber; + } + } + + const SequenceNumber_t &getSeqNumMax() const { + if (m_head == m_tail) { + return SEQUENCENUMBER_UNKNOWN; + } else { + return m_lastUsedSequenceNumber; + } + } + + void clear() { + m_head = 0; + m_tail = 0; + m_lastUsedSequenceNumber = {0, 0}; + } + +private: + std::array m_buffer{}; + uint16_t m_head = 0; + uint16_t m_tail = 0; + static_assert(sizeof(SIZE) <= sizeof(m_head), "Iterator is large enough for given size"); + + SequenceNumber_t m_lastUsedSequenceNumber{0, 0}; + + inline void incrementHead() { + incrementIterator(m_head); + if (m_head == m_tail) { + // Move without check + incrementIterator(m_tail); // drop one + } + } + + inline void incrementIterator(uint16_t &iterator) const { + ++iterator; + if (iterator >= m_buffer.size()) { + iterator = 0; + } + } + + inline void incrementTail() { + if (m_head != m_tail) { + incrementIterator(m_tail); + } + } + +protected: + // This constructor was created for unit testing + explicit SimpleHistoryCache(SequenceNumber_t lastUsed) + : SimpleHistoryCache() { + m_lastUsedSequenceNumber = lastUsed; + } +}; +} // namespace rtps + +#endif // PROJECT_SIMPLEHISTORYCACHE_H diff --git a/components/rtps_embedded/include/rtps/storages/ThreadSafeCircularBuffer.hpp b/components/rtps_embedded/include/rtps/storages/ThreadSafeCircularBuffer.hpp new file mode 100644 index 0000000000..fb0b95f155 --- /dev/null +++ b/components/rtps_embedded/include/rtps/storages/ThreadSafeCircularBuffer.hpp @@ -0,0 +1,76 @@ +/* +The MIT License +Copyright (c) 2019 Lehrstuhl Informatik 11 - RWTH Aachen University +Modifications Copyright (c) 2026 ATDev +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: +The above copyright notice and this permission notice shall be included in +all copies or substantial portions of the Software. +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +THE SOFTWARE + +This file is part of embeddedRTPS. + +Author: i11 - Embedded Software, RWTH Aachen University +*/ + +#ifndef RTPS_THREADSAFEQUEUE_H +#define RTPS_THREADSAFEQUEUE_H + +#include +#include +#include +#include + +namespace rtps { + +template class ThreadSafeCircularBuffer { + +public: + bool moveElementIntoBuffer(T &&elem); + bool copyElementIntoBuffer(const T &elem); + + /** + * Removes the first into the given hull. Also moves responsibility for + * resources. + * @return true if element was injected. False if no element was present. + */ + bool moveFirstInto(T &hull); + bool peakFirst(T &hull); + + uint32_t numElements(); + uint32_t insertionFailures(); + + void clear(); + +private: + std::array m_buffer{}; + uint16_t m_head = 0; + uint16_t m_tail = 0; + uint32_t m_num_elements = 0; + uint32_t m_insertion_failures = 0; + static_assert(SIZE + 1 < std::numeric_limits::max(), + "Iterator is large enough for given size"); + + std::mutex m_mutex; + + inline bool isFull() const; + inline void incrementIterator(uint16_t &iterator) const; + inline void incrementTail(); + inline void incrementHead(); +}; + +} // namespace rtps + +#include "ThreadSafeCircularBuffer.tpp" + +#endif // RTPS_THREADSAFEQUEUE_H diff --git a/components/rtps_embedded/include/rtps/storages/ThreadSafeCircularBuffer.tpp b/components/rtps_embedded/include/rtps/storages/ThreadSafeCircularBuffer.tpp new file mode 100644 index 0000000000..35c1bd9133 --- /dev/null +++ b/components/rtps_embedded/include/rtps/storages/ThreadSafeCircularBuffer.tpp @@ -0,0 +1,102 @@ + +#ifndef RTPS_THREADSAFECIRCULARBUFFER_TPP +#define RTPS_THREADSAFECIRCULARBUFFER_TPP + +namespace rtps { + +template +bool ThreadSafeCircularBuffer::moveElementIntoBuffer(T &&elem) { + std::lock_guard lock(m_mutex); + if (!isFull()) { + m_buffer[m_head] = std::move(elem); + incrementHead(); + return true; + } else { + m_insertion_failures++; + return false; + } +} + +template +bool ThreadSafeCircularBuffer::copyElementIntoBuffer(const T &elem) { + std::lock_guard lock(m_mutex); + if (!isFull()) { + m_buffer[m_head] = elem; + incrementHead(); + return true; + } else { + m_insertion_failures++; + return false; + } +} + +template +bool ThreadSafeCircularBuffer::moveFirstInto(T &hull) { + std::lock_guard lock(m_mutex); + if (m_head != m_tail) { + hull = std::move(m_buffer[m_tail]); + incrementTail(); + return true; + } else { + return false; + } +} + +template bool ThreadSafeCircularBuffer::peakFirst(T &hull) { + std::lock_guard lock(m_mutex); + if (m_head != m_tail) { + hull = m_buffer[m_tail]; + return true; + } else { + return false; + } +} + +template uint32_t ThreadSafeCircularBuffer::numElements() { + std::lock_guard lock(m_mutex); + return m_num_elements; +} + +template +uint32_t ThreadSafeCircularBuffer::insertionFailures() { + std::lock_guard lock(m_mutex); + return m_insertion_failures; +} + +template void ThreadSafeCircularBuffer::clear() { + std::lock_guard lock(m_mutex); + m_head = m_tail; + m_num_elements = 0; +} + +template bool ThreadSafeCircularBuffer::isFull() const { + auto it = m_head; + incrementIterator(it); + return it == m_tail; +} + +template +inline void ThreadSafeCircularBuffer::incrementIterator(uint16_t &iterator) const { + ++iterator; + if (iterator >= m_buffer.size()) { + iterator = 0; + } +} + +template +inline void ThreadSafeCircularBuffer::incrementTail() { + incrementIterator(m_tail); + m_num_elements--; +} + +template +inline void ThreadSafeCircularBuffer::incrementHead() { + incrementIterator(m_head); + m_num_elements++; + if (m_head == m_tail) { + incrementTail(); + } +} +} // namespace rtps + +#endif // RTPS_THREADSAFECIRCULARBUFFER_TPP diff --git a/components/rtps_embedded/include/rtps/utils/CdrBuffer.hpp b/components/rtps_embedded/include/rtps/utils/CdrBuffer.hpp new file mode 100644 index 0000000000..741be9a35d --- /dev/null +++ b/components/rtps_embedded/include/rtps/utils/CdrBuffer.hpp @@ -0,0 +1,86 @@ +/* +The MIT License +Copyright (c) 2026 ATDev +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: +The above copyright notice and this permission notice shall be included in +all copies or substantial portions of the Software. +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +THE SOFTWARE + +This file is part of embeddedRTPS. +*/ + +#ifndef RTPS_UTILS_CDRBUFFER_H +#define RTPS_UTILS_CDRBUFFER_H + +// Thin aliases/helpers over espp's `cdr` stream primitives that reproduce the +// exact byte behavior the engine previously got from the Micro-CDR library: +// - little-endian by default, +// - primitive writes/reads natural-aligned relative to the buffer start, +// - 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. + +#include +#include +#include +#include + +#include "cdr/stream.hpp" + +namespace rtps { + +using CdrSink = cdr::span_sink; +using CdrWriter = cdr::basic_writer; +using CdrReader = cdr::reader; + +inline std::span asWritableBytes(uint8_t *data, size_t size) { + return {reinterpret_cast(data), size}; +} + +inline std::span asBytes(const uint8_t *data, size_t size) { + return {reinterpret_cast(data), size}; +} + +/// Raw (unaligned) byte-array write (Micro-CDR's serialize_array_uint8_t). +inline void writeBytes(CdrWriter &writer, const uint8_t *data, size_t size) { + writer.write_bytes(asBytes(data, size)); +} + +/// Raw (unaligned) byte-array read (Micro-CDR's deserialize_array_uint8_t). +inline bool readBytes(CdrReader &reader, uint8_t *dst, size_t size) { + auto bytes = reader.read_bytes(size); + if (!bytes) { + return false; + } + std::memcpy(dst, bytes->data(), size); + return true; +} + +/// Advance the read position by up to `size` bytes, clamped to the end of the +/// buffer (Micro-CDR's advance_buffer). +inline void skipBytes(CdrReader &reader, size_t size) { + (void)reader.seek(std::min(reader.position() + size, reader.total_size())); +} + +/// Advance the read position to the next 4-byte boundary (parameter-list +/// element alignment), clamped to the end of the buffer (Micro-CDR's +/// align_to(buffer, 4)). +inline void alignTo4(CdrReader &reader) { + (void)reader.seek(std::min((reader.position() + 3) & ~size_t{3}, reader.total_size())); +} + +} // namespace rtps + +#endif // RTPS_UTILS_CDRBUFFER_H diff --git a/components/rtps_embedded/include/rtps/utils/Diagnostics.hpp b/components/rtps_embedded/include/rtps/utils/Diagnostics.hpp new file mode 100644 index 0000000000..1cc9e95504 --- /dev/null +++ b/components/rtps_embedded/include/rtps/utils/Diagnostics.hpp @@ -0,0 +1,86 @@ +/* +The MIT License +Copyright (c) 2019 Lehrstuhl Informatik 11 - RWTH Aachen University +Modifications Copyright (c) 2026 ATDev +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: +The above copyright notice and this permission notice shall be included in +all copies or substantial portions of the Software. +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +THE SOFTWARE + +This file is part of embeddedRTPS. + +Author: i11 - Embedded Software, RWTH Aachen University +*/ + +#ifndef RTPS_DIAGNOSTICS_H +#define RTPS_DIAGNOSTICS_H + +#include + +namespace rtps { +namespace Diagnostics { + +namespace ThreadPool { +extern uint32_t dropped_incoming_packets_usertraffic; +extern uint32_t dropped_incoming_packets_metatraffic; + +extern uint32_t dropped_outgoing_packets_usertraffic; +extern uint32_t dropped_outgoing_packets_metatraffic; + +extern uint32_t processed_incoming_metatraffic; +extern uint32_t processed_outgoing_metatraffic; +extern uint32_t processed_incoming_usertraffic; +extern uint32_t processed_outgoing_usertraffic; + +extern uint32_t max_ever_elements_outgoing_usertraffic_queue; +extern uint32_t max_ever_elements_incoming_usertraffic_queue; + +extern uint32_t max_ever_elements_outgoing_metatraffic_queue; +extern uint32_t max_ever_elements_incoming_metatraffic_queue; +} // namespace ThreadPool + +namespace StatefulReader { +extern uint32_t sfr_unexpected_sn; +extern uint32_t sfr_retransmit_requests; +} // namespace StatefulReader + +namespace Network { +extern uint32_t lwip_allocation_failures; +} + +namespace OS { +extern uint32_t current_free_heap_size; +} + +namespace SEDP { +extern uint32_t max_ever_remote_participants; +extern uint32_t current_remote_participants; + +extern uint32_t max_ever_matched_reader_proxies; +extern uint32_t current_max_matched_reader_proxies; + +extern uint32_t max_ever_matched_writer_proxies; +extern uint32_t current_max_matched_writer_proxies; + +extern uint32_t max_ever_unmatched_reader_proxies; +extern uint32_t current_max_unmatched_reader_proxies; + +extern uint32_t max_ever_unmatched_writer_proxies; +extern uint32_t current_max_unmatched_writer_proxies; +} // namespace SEDP + +} // namespace Diagnostics +} // namespace rtps + +#endif diff --git a/components/rtps_embedded/include/rtps/utils/Log.hpp b/components/rtps_embedded/include/rtps/utils/Log.hpp new file mode 100644 index 0000000000..ffcb35f0f4 --- /dev/null +++ b/components/rtps_embedded/include/rtps/utils/Log.hpp @@ -0,0 +1,48 @@ +/* +The MIT License +Copyright (c) 2019 Lehrstuhl Informatik 11 - RWTH Aachen University +Modifications Copyright (c) 2026 ATDev +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: +The above copyright notice and this permission notice shall be included in +all copies or substantial portions of the Software. +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +THE SOFTWARE + +This file is part of embeddedRTPS. + +Author: i11 - Embedded Software, RWTH Aachen University +*/ + +#ifndef RTPS_LOG_H +#define RTPS_LOG_H + +#include +#include + +#define RTPS_GLOBAL_VERBOSE 0 + +#define SFW_VERBOSE 1 +#define SPDP_VERBOSE 0 +#define PBUF_WRAP_VERBOSE 0 +#define SEDP_VERBOSE 1 +#define RECV_VERBOSE 1 +#define PARTICIPANT_VERBOSE 0 +#define DOMAIN_VERBOSE 1 +#define UDP_DRIVER_VERBOSE 1 +#define TSCB_VERBOSE 1 +#define SLW_VERBOSE 0 +#define SFR_VERBOSE 1 +#define SLR_VERBOSE 1 +#define THREAD_POOL_VERBOSE 1 + +#endif // RTPS_LOG_H diff --git a/components/rtps_embedded/include/rtps/utils/constants.hpp b/components/rtps_embedded/include/rtps/utils/constants.hpp new file mode 100644 index 0000000000..58b5c7c741 --- /dev/null +++ b/components/rtps_embedded/include/rtps/utils/constants.hpp @@ -0,0 +1,29 @@ +/* +The MIT License +Copyright (c) 2019 Lehrstuhl Informatik 11 - RWTH Aachen University +Modifications Copyright (c) 2026 ATDev +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: +The above copyright notice and this permission notice shall be included in +all copies or substantial portions of the Software. +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +THE SOFTWARE + +This file is part of embeddedRTPS. + +Author: i11 - Embedded Software, RWTH Aachen University +*/ + +#ifndef RTPS_CONSTANTS_H +#define RTPS_CONSTANTS_H + +#endif // RTPS_CONSTANTS_H diff --git a/components/rtps_embedded/include/rtps/utils/hash.hpp b/components/rtps_embedded/include/rtps/utils/hash.hpp new file mode 100644 index 0000000000..2cd6ae5e6a --- /dev/null +++ b/components/rtps_embedded/include/rtps/utils/hash.hpp @@ -0,0 +1,42 @@ +/* +The MIT License +Copyright (c) 2019 Lehrstuhl Informatik 11 - RWTH Aachen University +Modifications Copyright (c) 2026 ATDev +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: +The above copyright notice and this permission notice shall be included in +all copies or substantial portions of the Software. +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +THE SOFTWARE + +This file is part of embeddedRTPS. + +Author: i11 - Embedded Software, RWTH Aachen University +*/ + +#ifndef RTPS_HASH_H +#define RTPS_HASH_H + +#include + +namespace rtps { +inline size_t hashCharArray(const char *p, size_t s) { + size_t result = 0; + const size_t prime = 31; + for (size_t i = 0; i < s; ++i) { + result = p[i] + (result * prime); + } + return result; +} +} // namespace rtps + +#endif diff --git a/components/rtps_embedded/include/rtps/utils/printutils.hpp b/components/rtps_embedded/include/rtps/utils/printutils.hpp new file mode 100644 index 0000000000..eae1a22077 --- /dev/null +++ b/components/rtps_embedded/include/rtps/utils/printutils.hpp @@ -0,0 +1,30 @@ +// +// Created by andreas on 13.01.19. +// + +#ifndef RTPS_PRINTUTILS_H +#define RTPS_PRINTUTILS_H + +#include "rtps/common/types.hpp" + +inline void printEntityId(rtps::EntityId_t id) { + for (const auto byte : id.entityKey) { + printf("%x", (int)byte); + } + printf("%x", static_cast(id.entityKind)); + printf("\n"); +} + +inline void printGuidPrefix(rtps::GuidPrefix_t prefix) { + for (const auto byte : prefix.id) { + printf("%x", (int)byte); + } +} + +inline void printGuid(rtps::Guid_t guid) { + printGuidPrefix(guid.prefix); + printf(":"); + printEntityId(guid.entityId); +} + +#endif // RTPS_PRINTUTILS_H diff --git a/components/rtps_embedded/include/rtps/utils/sysFunctions.hpp b/components/rtps_embedded/include/rtps/utils/sysFunctions.hpp new file mode 100644 index 0000000000..9801a539b3 --- /dev/null +++ b/components/rtps_embedded/include/rtps/utils/sysFunctions.hpp @@ -0,0 +1,46 @@ +/* +The MIT License +Copyright (c) 2019 Lehrstuhl Informatik 11 - RWTH Aachen University +Modifications Copyright (c) 2026 ATDev +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: +The above copyright notice and this permission notice shall be included in +all copies or substantial portions of the Software. +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +THE SOFTWARE + +This file is part of embeddedRTPS. + +Author: i11 - Embedded Software, RWTH Aachen University +*/ + +#ifndef PROJECT_SYSFUNCTIONS_H +#define PROJECT_SYSFUNCTIONS_H + +#include "rtps/common/types.hpp" + +#include + +namespace rtps { +inline Time_t getCurrentTimeStamp() { + static const auto start = std::chrono::steady_clock::now(); + const auto elapsed = std::chrono::steady_clock::now() - start; + const auto nowMs = std::chrono::duration_cast(elapsed).count(); + + Time_t now; + now.seconds = static_cast(nowMs / 1000); + now.fraction = static_cast((nowMs % 1000) * ((1ULL << 32) / 1000)); + return now; +} +} // namespace rtps + +#endif // PROJECT_SYSFUNCTIONS_H diff --git a/components/rtps_embedded/include/rtps/utils/udpUtils.hpp b/components/rtps_embedded/include/rtps/utils/udpUtils.hpp new file mode 100644 index 0000000000..aed64ed096 --- /dev/null +++ b/components/rtps_embedded/include/rtps/utils/udpUtils.hpp @@ -0,0 +1,114 @@ +/* +The MIT License +Copyright (c) 2019 Lehrstuhl Informatik 11 - RWTH Aachen University +Modifications Copyright (c) 2026 ATDev +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: +The above copyright notice and this permission notice shall be included in +all copies or substantial portions of the Software. +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +THE SOFTWARE + +This file is part of embeddedRTPS. + +Author: i11 - Embedded Software, RWTH Aachen University +*/ + +#ifndef RTPS_UDP_UTILS_H +#define RTPS_UDP_UTILS_H + +#include "rtps/config.hpp" + +#include + +namespace rtps { +namespace { +const uint16_t PB = 7400; // Port Base Number +const uint16_t DG = 250; // DomainId Gain +const uint16_t PG = 2; // ParticipantId Gain +// Additional Offsets +const uint16_t D0 = 0; // Builtin multicast +const uint16_t D1 = 10; // Builtin unicast +const uint16_t D2 = 1; // User multicast +const uint16_t D3 = 11; // User unicast +} // namespace + +constexpr std::array transformIP4ToU32(uint8_t MSB, uint8_t p2, uint8_t p1, + uint8_t LSB) { + return {MSB, p2, p1, LSB}; +} + +constexpr Ip4Port_t getBuiltInUnicastPort(ParticipantId_t participantId) { + return PB + DG * Config::DOMAIN_ID + D1 + PG * participantId; +} + +constexpr Ip4Port_t getBuiltInMulticastPort() { return PB + DG * Config::DOMAIN_ID + D0; } + +constexpr Ip4Port_t getUserUnicastPort(ParticipantId_t participantId) { + return PB + DG * Config::DOMAIN_ID + D3 + PG * participantId; +} + +constexpr Ip4Port_t getUserMulticastPort() { return PB + DG * Config::DOMAIN_ID + D2; } + +constexpr bool isUserPort(Ip4Port_t port) { + return (port & 1) == 1; +} // really useful? There may be other user ports than just odd ones? + +inline bool isMultiCastPort(Ip4Port_t port) { + const auto idWithoutBase = port - PB - DG * Config::DOMAIN_ID; + return idWithoutBase == D0 || + (idWithoutBase >= D2 && idWithoutBase < D3); // There are several UserMulticastPorts! +} + +inline bool isMetaMultiCastPort(Ip4Port_t port) { + const auto idWithoutBase = port - PB - DG * Config::DOMAIN_ID; + return idWithoutBase == D0; +} + +inline bool isUserMultiCastPort(Ip4Port_t port) { + const auto idWithoutBase = port - PB - DG * Config::DOMAIN_ID; + return (idWithoutBase >= D2 && idWithoutBase < D1); +} + +inline bool isZeroAddress(const std::array &address) { + return address[0] == 0 && address[1] == 0 && address[2] == 0 && address[3] == 0; +} + +inline bool isMulticastAddress(const std::array &address) { + return address[0] >= 224 && address[0] <= 239; +} + +inline ParticipantId_t getParticipantIdFromUnicastPort(Ip4Port_t port, bool isUserPort) { + + const auto basePart = PB + DG * Config::DOMAIN_ID; + ParticipantId_t participantPart = port - basePart; + + uint16_t offset = 0; + if (isUserPort) { + offset = D3; + } else { + offset = D1; + } + + participantPart -= offset; + + auto id = static_cast(participantPart / PG); + if (id * PG + basePart + offset == port) { + return id; + } else { + return PARTICIPANT_ID_INVALID; + } +} + +} // namespace rtps + +#endif // RTPS_UDP_H diff --git a/components/rtps_embedded/include/rtps_participant.hpp b/components/rtps_embedded/include/rtps_participant.hpp new file mode 100644 index 0000000000..eb27504551 --- /dev/null +++ b/components/rtps_embedded/include/rtps_participant.hpp @@ -0,0 +1,167 @@ +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "base_component.hpp" + +// Forward declarations of the embeddedRTPS engine types (see +// components/rtps_embedded/include/rtps/). The engine headers are only needed +// by the implementation; users of this facade never touch them directly. +namespace rtps { +class Domain; +class Participant; +class Writer; +class Reader; +class ReaderCacheChange; +} // namespace rtps + +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). +/// 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 +/// callback as CDR-encapsulated payload bytes (use the reflection-driven `cdr` +/// component - cdr::serialize / cdr::deserialize - to (de)serialize them). +/// +/// For ROS 2 interop, use ROS 2 naming conventions: topic "rt/" and type +/// "::msg::dds_::_" (e.g. topic "rt/chatter" with type +/// "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 +/// beneath is unchanged, so its current limitations apply - domain id is fixed +/// at compile time (Config::DOMAIN_ID, default 0), announcement/heartbeat +/// periods are compile-time constants, endpoint counts are bounded by the +/// engine's pools, and a second RtpsParticipant in the same process will +/// collide on unicast ports (scheduled fix in Phase 2). +class RtpsParticipant : public BaseComponent { +public: + /// Callback for samples received on a reader. The span holds the + /// CDR-encapsulated payload (4-byte encapsulation header + CDR body) and is + /// only valid for the duration of the callback; copy it if you keep it. + /// \note Runs on an engine worker thread - return quickly, do not block. + using sample_callback_t = std::function cdr_payload)>; + + /// Callback invoked when a remote endpoint matches one of this participant's + /// writers (publisher matched) or readers (subscriber matched). + /// \note Runs on an engine worker thread - return quickly, do not block. + using matched_callback_t = std::function; + + /// Reliability QoS for a writer or reader. + enum class Reliability { + BEST_EFFORT, ///< Fire-and-forget delivery (stateless endpoint). + RELIABLE, ///< HEARTBEAT/ACKNACK acknowledged delivery (stateful endpoint). + }; + + /// Configuration for a writer (publishing endpoint). + struct WriterConfig { + std::string topic; ///< DDS topic name (e.g. "rt/chatter" for ROS 2). + std::string type_name; ///< DDS type name (e.g. "std_msgs::msg::dds_::String_"). + Reliability reliability{Reliability::BEST_EFFORT}; ///< Reliability QoS. + }; + + /// Configuration for a reader (subscribing endpoint). + struct ReaderConfig { + std::string topic; ///< DDS topic name (e.g. "rt/chatter" for ROS 2). + std::string type_name; ///< DDS type name (e.g. "std_msgs::msg::dds_::String_"). + Reliability reliability{Reliability::BEST_EFFORT}; ///< Reliability QoS. + sample_callback_t on_sample{nullptr}; ///< Called for each received sample. + }; + + /// Configuration for the participant. + struct Config { + /// IPv4 address of the network interface to use. On the host, leave empty + /// to auto-detect the first non-loopback IPv4 interface. On ESP targets it + /// must be set explicitly (e.g. from the WiFi/Ethernet netif IP). + std::string interface_address{}; + matched_callback_t on_publisher_matched{nullptr}; ///< A writer gained a remote reader. + matched_callback_t on_subscriber_matched{nullptr}; ///< A reader gained a remote writer. + Logger::Verbosity log_level{Logger::Verbosity::WARN}; ///< Facade log verbosity. + }; + + /// Construct the participant (does not open sockets; see start()). + /// \param config The participant configuration. + explicit RtpsParticipant(const Config &config); + + /// Stops the participant (see stop()). + ~RtpsParticipant(); + + RtpsParticipant(const RtpsParticipant &) = delete; + RtpsParticipant &operator=(const RtpsParticipant &) = delete; + RtpsParticipant(RtpsParticipant &&) = delete; + RtpsParticipant &operator=(RtpsParticipant &&) = delete; + + /// Start the participant: bring up the RTPS transport and begin SPDP/SEDP + /// discovery. Writers and readers can only be added after a successful + /// start(). + /// \return True on success (false if already started or bring-up failed). + bool start(); + + /// Stop the participant and its discovery/transport threads. Registered + /// callbacks will not be invoked after stop() returns. + void stop(); + + /// \return True if the participant has been started and not stopped. + bool is_started() const { return started_; } + + /// Add a publishing endpoint. + /// \param config The writer configuration. + /// \return True on success (false when not started, on duplicate topic, or + /// when the engine's writer pool is exhausted). + bool add_writer(const WriterConfig &config); + + /// Add a subscribing endpoint. + /// \param config The reader configuration. + /// \return True on success (false when not started or when the engine's + /// reader pool is exhausted). + bool add_reader(const ReaderConfig &config); + + /// Publish a CDR-encapsulated sample on a topic previously registered with + /// add_writer(). + /// \param topic The topic name used in add_writer(). + /// \param cdr_payload The CDR-encapsulated sample (4-byte encapsulation + /// header + CDR body); copied into the writer's history. + /// \return True if the sample was accepted into the writer history. + bool publish(std::string_view topic, std::span cdr_payload); + +protected: + /// Per-reader context bridging the engine's C function-pointer callback to + /// the std::function callback; heap-allocated so its address stays stable + /// for the lifetime of the reader. + struct ReaderContext { + RtpsParticipant *self{nullptr}; + sample_callback_t on_sample{nullptr}; + std::mutex buffer_mutex; + std::vector buffer; + }; + + static void reader_trampoline(void *arg, const rtps::ReaderCacheChange &change); + static void publisher_matched_trampoline(void *arg); + static void subscriber_matched_trampoline(void *arg); + + bool resolve_interface_address(std::array &ip_bytes) const; + + Config config_; + std::atomic started_{false}; + mutable std::mutex mutex_; ///< guards domain_/participant_/writers_/reader_contexts_ + std::unique_ptr domain_; + rtps::Participant *participant_{nullptr}; + std::unordered_map writers_; + std::vector> reader_contexts_; +}; + +} // namespace espp diff --git a/components/rtps_embedded/interop/Dockerfile b/components/rtps_embedded/interop/Dockerfile new file mode 100644 index 0000000000..459fcbc433 --- /dev/null +++ b/components/rtps_embedded/interop/Dockerfile @@ -0,0 +1,13 @@ +# FastDDS / ROS 2 interop harness image for the embeddedRTPS engine. +# ros:jazzy-ros-base ships FastDDS + rmw_fastrtps + the ros2 CLI, giving both the +# raw-DDS and ROS 2 sides of the interop matrix in one image. The espp repo is +# bind-mounted read-only at /work, copied to a container-local tree, and built +# there (the repo's lib/pc install dir carries the developer's host-platform +# artifacts and must not be clobbered with linux ones). See run_interop.sh. +FROM ros:jazzy-ros-base + +RUN apt-get update && apt-get install -y --no-install-recommends \ + build-essential cmake git rsync python3-dev pybind11-dev \ + && rm -rf /var/lib/apt/lists/* + +WORKDIR /work diff --git a/components/rtps_embedded/interop/README.md b/components/rtps_embedded/interop/README.md new file mode 100644 index 0000000000..55eefce560 --- /dev/null +++ b/components/rtps_embedded/interop/README.md @@ -0,0 +1,36 @@ +# RTPS interop harness (FastDDS / ROS 2) + +The Phase 0 safety net from `../REFACTOR_PLAN.md`: every refactor phase must keep +this matrix green. + +## Run + +```bash +cd components/rtps_embedded/interop +./run.sh +``` + +Requires docker. One container (`ros:jazzy-ros-base` = FastDDS + rmw_fastrtps + +`ros2` CLI) runs everything in a single network namespace, so RTPS multicast works +unconditionally. The repo is bind-mounted and copied to a container-local tree +before building, so your host `lib/pc` artifacts are never touched. + +## Matrix + +| test | what it proves | +|---|---| +| build | the engine + espp lib build on linux | +| golden | wire-format bytes unchanged (see `pc/tests/rtps_embedded_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 | +| espp_be_pub->ros2_be_echo | best-effort pairing | + +## Notes + +- The engine probes participant ids for free unicast ports (binding with reuse + disabled), so multiple espp participants can run on one host and the start + order does not matter. +- Received datagrams are dispatched via `espp::SocketReactor` onto a shared + worker pool; there is no dedicated per-socket receive task and no engine-owned + packet queue. diff --git a/components/rtps_embedded/interop/run.sh b/components/rtps_embedded/interop/run.sh new file mode 100755 index 0000000000..ddb64667a4 --- /dev/null +++ b/components/rtps_embedded/interop/run.sh @@ -0,0 +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) +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 diff --git a/components/rtps_embedded/interop/run_interop.sh b/components/rtps_embedded/interop/run_interop.sh new file mode 100755 index 0000000000..4f1cee4cb4 --- /dev/null +++ b/components/rtps_embedded/interop/run_interop.sh @@ -0,0 +1,97 @@ +#!/usr/bin/env bash +# FastDDS / ROS 2 interop test for the embeddedRTPS engine (runs INSIDE the +# harness container; use ./run.sh from the host). +# +# All peers share one network namespace so RTPS multicast works unconditionally. +# +# NOTE: no `set -u` - ROS 2's setup.bash references unset variables. + +# Work on a container-local copy: the pc tests link the lib installed into +# lib/pc inside the source tree, which on the bind mount holds the developer's +# host-platform (e.g. macOS) artifacts. Building in-place would either link +# incompatible objects or clobber them with linux ones. +echo "===== Copy sources to container-local tree =====" +rsync -a --delete --exclude '.git/' --exclude 'build/' --exclude 'build-*/' --exclude 'managed_components/' --exclude 'docs/' --exclude 'dependencies.lock' /work/ /tmp/espp/ +cd /tmp/espp + +PASS=0 +FAIL=0 +note() { echo -e "\n===== $* ====="; } +result() { # name exit_code + if [ "$2" -eq 0 ]; then echo "RESULT PASS: $1"; PASS=$((PASS+1)); + else echo "RESULT FAIL: $1"; FAIL=$((FAIL+1)); fi +} + +note "Build espp lib + host binaries (linux)" +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_embedded_interop_pub rtps_embedded_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 +BIN=pc/build + +source /opt/ros/jazzy/setup.bash +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" $? + +note "espp <-> espp in-process loopback" +"$BIN"/rtps_embedded_pubsub; result "loopback" $? + +note "facade <-> facade in-process (two participants, port probing)" +"$BIN"/rtps_facade_pubsub; result "facade_loopback" $? + +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 & +XSUB=$! +sleep 2 +"$BIN"/rtps_embedded_interop_pub xproc std_msgs::msg::dds_::String_ 0 30 200 > /tmp/xpub.log 2>&1 & +XPUB=$! +wait $XSUB +xsub_rc=$? +kill $XPUB 2>/dev/null; wait $XPUB 2>/dev/null +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 & +ESPP_PID=$! +sleep 3 +timeout 30 ros2 topic echo --once /chatter std_msgs/msg/String > /tmp/echo.log 2>&1 +echo_rc=$? +kill $ESPP_PID 2>/dev/null; wait $ESPP_PID 2>/dev/null +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 & +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 & +ROS_PID=$! +wait $SUB_PID +sub_rc=$? +kill $ROS_PID 2>/dev/null; wait $ROS_PID 2>/dev/null +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 & +ESPP_PID=$! +sleep 3 +timeout 30 ros2 topic echo --once --qos-reliability best_effort /chatter std_msgs/msg/String > /tmp/echo2.log 2>&1 +echo2_rc=$? +kill $ESPP_PID 2>/dev/null; wait $ESPP_PID 2>/dev/null +result "espp_be_pub->ros2_be_echo" $echo2_rc + +echo "" +echo "==================== SUMMARY ====================" +echo "PASS=$PASS FAIL=$FAIL" +[ $FAIL -eq 0 ] && echo "INTEROP PASS" || echo "INTEROP FAIL" +exit $FAIL diff --git a/components/rtps_embedded/src/communication/EsppTransport.cpp b/components/rtps_embedded/src/communication/EsppTransport.cpp new file mode 100644 index 0000000000..f270e28199 --- /dev/null +++ b/components/rtps_embedded/src/communication/EsppTransport.cpp @@ -0,0 +1,306 @@ +/* +The MIT License +Copyright (c) 2019 Lehrstuhl Informatik 11 - RWTH Aachen University +Modifications Copyright (c) 2026 ATDev +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: +The above copyright notice and this permission notice shall be included in +all copies or substantial portions of the Software. +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +THE SOFTWARE + +This file is part of embeddedRTPS. + +Author: i11 - Embedded Software, RWTH Aachen University +*/ + +#include "rtps/communication/EsppTransport.hpp" + +#include "task.hpp" + +#include +#include +#include +#include +#include + +using rtps::EsppTransport; + +namespace { + +bool parseIp4Address(const std::string &address, rtps::Ip4AddressBytes &out) { + rtps::Ip4AddressBytes parsed{0, 0, 0, 0}; + const char *cursor = address.c_str(); + + for (std::size_t i = 0; i < parsed.size(); ++i) { + char *end = nullptr; + const unsigned long value = std::strtoul(cursor, &end, 10); + if (end == cursor || value > 255) { + return false; + } + + parsed[i] = static_cast(value); + if (i + 1 < parsed.size()) { + if (*end != '.') { + return false; + } + cursor = end + 1; + } else if (*end != '\0') { + return false; + } + } + + out = parsed; + return true; +} + +bool isMulticastAddress(const rtps::Ip4AddressBytes &addr) { + return addr[0] >= 224 && addr[0] <= 239; +} + +} // namespace + +EsppTransport::EsppTransport(RxCallback callback, void *args) + : espp::BaseComponent("RtpsTransport", espp::Logger::Verbosity::WARN) + , m_rxCallback(callback) + , m_callbackArgs(args) { + espp::ThreadPool::Config pool_config; + pool_config.worker_count = 2; + pool_config.worker_task_config = { + .name = "rtps_worker", + .stack_size_bytes = Config::THREAD_POOL_READER_STACKSIZE, + .priority = Config::THREAD_POOL_READER_PRIO, + }; + m_pool = std::make_shared(pool_config); + + espp::SocketReactor::Config reactor_config; + reactor_config.thread_pool = m_pool; + reactor_config.loop_task_config = { + .name = "rtps_reactor", + .stack_size_bytes = Config::THREAD_POOL_READER_STACKSIZE, + .priority = Config::THREAD_POOL_READER_PRIO, + }; + reactor_config.log_level = espp::Logger::Verbosity::WARN; + m_reactor = std::make_shared(reactor_config); +} + +EsppTransport::Channel *EsppTransport::findChannel(Ip4Port_t port) { + auto it = std::find_if(m_channels.begin(), m_channels.end(), [port](const auto &channel) { + return channel.in_use && channel.port == port; + }); + return (it != m_channels.end()) ? &(*it) : nullptr; +} + +const EsppTransport::Channel *EsppTransport::findChannel(Ip4Port_t port) const { + auto it = std::find_if(m_channels.begin(), m_channels.end(), [port](const auto &channel) { + return channel.in_use && channel.port == port; + }); + return (it != m_channels.end()) ? &(*it) : nullptr; +} + +std::string EsppTransport::ip4ToString(const Ip4AddressBytes &addr) { + return std::to_string(addr[0]) + "." + std::to_string(addr[1]) + "." + std::to_string(addr[2]) + + "." + std::to_string(addr[3]); +} + +bool EsppTransport::startReceiver(Channel &channel, Ip4Port_t receivePort) { + if (!channel.socket) { + logger_.error("startReceiver called with null socket on port {}", receivePort); + return false; + } + + // Register the socket with the shared reactor instead of spawning a + // dedicated blocking-recv task per port: add_udp_receiver() binds the socket + // per the config and dispatches each datagram onto the reactor's worker + // pool. One-shot arming guarantees at most one in-flight handler per socket, + // preserving RTPS's per-locator ordering. + espp::UdpSocket::ReceiveConfig receive_config; + receive_config.port = receivePort; + receive_config.buffer_size = 1024 * 8; + receive_config.on_receive_callback = + [this, receivePort](std::vector &data, + const espp::Socket::Info &sender) -> std::optional> { + onReceive(receivePort, data, sender); + return std::nullopt; + }; + + channel.reactor_id = m_reactor->add_udp_receiver(*channel.socket, receive_config); + if (channel.reactor_id == espp::SocketReactor::INVALID_ID) { + logger_.error("Failed to register UDP receiver on port {} with the reactor", receivePort); + return false; + } + return true; +} + +EsppTransport::Channel *EsppTransport::createChannel(Ip4Port_t receivePort, bool allow_reuse) { + for (auto &channel : m_channels) { + if (channel.in_use) { + continue; + } + + espp::UdpSocket::Config socket_config; + socket_config.log_level = espp::Logger::Verbosity::WARN; + channel.socket = std::make_unique(socket_config); + if (!channel.socket->is_valid()) { + logger_.error("Failed to create valid UDP socket for port {}", receivePort); + channel.socket.reset(); + return nullptr; + } + + if (!allow_reuse && !channel.socket->disable_reuse()) { + logger_.error("Failed to disable port reuse for unicast port {}", receivePort); + channel.socket.reset(); + return nullptr; + } + + channel.port = receivePort; + channel.in_use = true; + + if (!startReceiver(channel, receivePort)) { + channel.socket.reset(); + channel.port = 0; + channel.in_use = false; + return nullptr; + } + + for (const auto &group : m_multicastGroups) { + (void)channel.socket->add_multicast_group(group); + } + return &channel; + } + return nullptr; +} + +void EsppTransport::onReceive(Ip4Port_t receivePort, std::vector &data, + const espp::Socket::Info &sender) const { + if (m_rxCallback == nullptr) { + return; + } + + Ip4AddressBytes remoteAddress{0, 0, 0, 0}; + if (!parseIp4Address(sender.address, remoteAddress)) { + logger_.warn("Could not parse sender IPv4 address '{}', using 0.0.0.0", sender.address); + } + logger_.debug("received {} bytes on port {}", static_cast(data.size()), + receivePort); + m_rxCallback(m_callbackArgs, data.data(), data.size(), receivePort, + static_cast(sender.port), remoteAddress); +} + +bool EsppTransport::submit(std::function job) { + if (!m_pool || !m_pool->try_submit(std::move(job))) { + logger_.warn("Transport worker pool rejected a job (stopped or queue full)"); + return false; + } + return true; +} + +void EsppTransport::stop() { + if (m_reactor) { + m_reactor->stop(); + } + if (m_pool) { + m_pool->stop(); + } +} + +bool EsppTransport::ensureReceivePort(Ip4Port_t receivePort, bool is_multicast) { + std::lock_guard lock(m_mutex); + + Channel *existing = findChannel(receivePort); + if (existing != nullptr) { + return true; + } + + Channel *created = createChannel(receivePort, /*allow_reuse=*/is_multicast); + return created != nullptr; +} + +bool EsppTransport::releaseReceivePort(Ip4Port_t receivePort) { + std::lock_guard lock(m_mutex); + Channel *channel = findChannel(receivePort); + if (channel == nullptr) { + return false; + } + if (channel->reactor_id != espp::SocketReactor::INVALID_ID) { + m_reactor->remove(channel->reactor_id); + channel->reactor_id = espp::SocketReactor::INVALID_ID; + } + channel->socket.reset(); + channel->port = 0; + channel->in_use = false; + return true; +} + +bool EsppTransport::joinMultiCastGroup(const Ip4AddressBytes &addr) const { + std::lock_guard lock(m_mutex); + + const std::string group = ip4ToString(addr); + const bool already_joined = + std::any_of(m_multicastGroups.begin(), m_multicastGroups.end(), + [&](const auto &existing_group) { return existing_group == group; }); + if (already_joined) { + return true; + } + + bool any_joined = false; + bool has_active_channels = false; + for (auto &channel : m_channels) { + if (!channel.in_use || !channel.socket) { + continue; + } + has_active_channels = true; + any_joined = channel.socket->add_multicast_group(group) || any_joined; + } + + if (!has_active_channels) { + // No active sockets yet: defer join until channels are created. + m_multicastGroups.push_back(group); + return true; + } + + if (any_joined) { + m_multicastGroups.push_back(group); + return true; + } + + logger_.warn("Failed to join multicast group {} on all active channels", group); + return false; +} + +void EsppTransport::sendPacket(PacketInfo &info) { + std::lock_guard lock(m_mutex); + + Channel *channel = findChannel(info.srcPort); + if (channel == nullptr) { + // Sending from one of our own unicast ports: apply unicast semantics + // (no port sharing) if the channel was not already registered. + channel = createChannel(info.srcPort, /*allow_reuse=*/false); + } + + if (channel == nullptr || !channel->socket) { + logger_.error("No UDP channel available for source port {}", info.srcPort); + return; + } + + if (info.payload.empty()) { + return; + } + + espp::UdpSocket::SendConfig send_config; + const Ip4AddressBytes destination = info.destAddr; + send_config.ip_address = ip4ToString(destination); + send_config.port = info.destPort; + send_config.is_multicast_endpoint = isMulticastAddress(info.destAddr); + + (void)channel->socket->send(info.payload, send_config); +} diff --git a/components/rtps_embedded/src/discovery/ParticipantProxyData.cpp b/components/rtps_embedded/src/discovery/ParticipantProxyData.cpp new file mode 100644 index 0000000000..68db3c327d --- /dev/null +++ b/components/rtps_embedded/src/discovery/ParticipantProxyData.cpp @@ -0,0 +1,258 @@ +/* +The MIT License +Copyright (c) 2019 Lehrstuhl Informatik 11 - RWTH Aachen University +Modifications Copyright (c) 2026 ATDev +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: +The above copyright notice and this permission notice shall be included in +all copies or substantial portions of the Software. +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +THE SOFTWARE + +This file is part of embeddedRTPS. + +Author: i11 - Embedded Software, RWTH Aachen University +*/ + +#include "rtps/discovery/ParticipantProxyData.hpp" +#include "rtps/entities/Participant.hpp" +#include "rtps/utils/Log.hpp" + +using rtps::ParticipantProxyData; + +#if SPDP_VERBOSE && RTPS_GLOBAL_VERBOSE +#define PPD_LOG(...) logger_.warn(__VA_ARGS__) +#else +#define PPD_LOG(...) \ + do { \ + } while (0) +#endif + +ParticipantProxyData::ParticipantProxyData(Guid_t guid) + : espp::BaseComponent("RtpsParticipantProxy", espp::Logger::Verbosity::WARN) + , m_guid(guid) {} + +void ParticipantProxyData::reset() { + m_guid = Guid_t{GUIDPREFIX_UNKNOWN, ENTITYID_UNKNOWN}; + m_manualLivelinessCount = Count_t{1}; + m_expectsInlineQos = false; + onAliveSignal(); + for (int i = 0; i < Config::SPDP_MAX_NUM_LOCATORS; ++i) { + m_metatrafficUnicastLocatorList[i].setInvalid(); + m_metatrafficMulticastLocatorList[i].setInvalid(); + m_defaultUnicastLocatorList[i].setInvalid(); + m_defaultMulticastLocatorList[i].setInvalid(); + } +} + +bool ParticipantProxyData::readFromBuffer(CdrReader &buffer, Participant *participant) { + reset(); + PPD_LOG("Start deserializing ParticipantProxyData"); + PPD_LOG("Buffer has {} bytes remaining", buffer.remaining()); + while (buffer.remaining() >= 4) { + const auto pidRaw = buffer.read(); + const auto lengthRaw = buffer.read(); + if (!pidRaw || !lengthRaw) { + return false; + } + const auto pid = static_cast(*pidRaw); + const uint16_t length = *lengthRaw; + PPD_LOG("Deserializing parameter with id {} and length {}", static_cast(pid), length); + if (buffer.remaining() < length) { + PPD_LOG("Not enough data left in buffer to read parameter with id {} and length {}", + static_cast(pid), length); + return false; + } + + switch (pid) { + case ParameterId::PID_KEY_HASH: { + // TODO + break; + } + + case ParameterId::PID_PROTOCOL_VERSION: { + const auto major = buffer.read(); + if (!major) { + return false; + } + m_protocolVersion.major = *major; + if (m_protocolVersion.major < PROTOCOLVERSION.major) { + PPD_LOG("Unsupported protocol version: {}.{}", m_protocolVersion.major, + m_protocolVersion.minor); + return false; + } else { + const auto minor = buffer.read(); + if (!minor) { + return false; + } + m_protocolVersion.minor = *minor; + } + PPD_LOG("Protocol version: {}.{}", m_protocolVersion.major, m_protocolVersion.minor); + break; + } + case ParameterId::PID_VENDORID: { + if (!readBytes(buffer, m_vendorId.vendorId.data(), m_vendorId.vendorId.size())) { + return false; + } + PPD_LOG("vendor id struct size: {}", m_vendorId.vendorId.size()); + PPD_LOG("Vendor ID: {} {}", m_vendorId.vendorId[0], m_vendorId.vendorId[1]); + break; + } + + case ParameterId::PID_EXPECTS_INLINE_QOS: { + const auto flag = buffer.read(); + if (!flag) { + return false; + } + m_expectsInlineQos = (*flag != 0); + break; + } + case ParameterId::PID_PARTICIPANT_GUID: { + if (!readBytes(buffer, m_guid.prefix.id.data(), m_guid.prefix.id.size()) || + !readBytes(buffer, m_guid.entityId.entityKey.data(), m_guid.entityId.entityKey.size())) { + return false; + } + const auto kind = buffer.read(); + if (!kind) { + return false; + } + m_guid.entityId.entityKind = static_cast(*kind); + if (participant->findRemoteParticipant(m_guid.prefix)) { + PPD_LOG("stopping deserialization early, participant is known"); + return true; + } + PPD_LOG("Participant GUID: {} {} {} {} {} {} {} {} {} {}", m_guid.prefix.id[0], + m_guid.prefix.id[1], m_guid.prefix.id[2], m_guid.prefix.id[3], m_guid.prefix.id[4], + m_guid.prefix.id[5], m_guid.prefix.id[6], m_guid.prefix.id[7], m_guid.prefix.id[8], + m_guid.prefix.id[9]); + break; + } + case ParameterId::PID_METATRAFFIC_MULTICAST_LOCATOR: { + if (!readLocatorIntoList(buffer, m_metatrafficMulticastLocatorList)) { + PPD_LOG("Failed to read metatraffic multicast locator"); + return false; + } + break; + } + case ParameterId::PID_METATRAFFIC_UNICAST_LOCATOR: { + if (!readLocatorIntoList(buffer, m_metatrafficUnicastLocatorList)) { + PPD_LOG("Failed to read metatraffic unicast locator"); + return false; + } + break; + } + case ParameterId::PID_DEFAULT_UNICAST_LOCATOR: { + if (!readLocatorIntoList(buffer, m_defaultUnicastLocatorList)) { + PPD_LOG("Failed to read default unicast locator"); + return false; + } + break; + } + case ParameterId::PID_DEFAULT_MULTICAST_LOCATOR: { + if (!readLocatorIntoList(buffer, m_defaultMulticastLocatorList)) { + PPD_LOG("Failed to read default multicast locator"); + return false; + } + break; + } + case ParameterId::PID_PARTICIPANT_LEASE_DURATION: { + const auto seconds = buffer.read(); + const auto fraction = buffer.read(); + if (!seconds || !fraction) { + return false; + } + m_leaseDuration.seconds = *seconds; + m_leaseDuration.fraction = *fraction; + break; + } + case ParameterId::PID_BUILTIN_ENDPOINT_SET: { + const auto endpointSet = buffer.read(); + if (!endpointSet) { + return false; + } + m_availableBuiltInEndpoints = *endpointSet; + break; + } + case ParameterId::PID_ENTITY_NAME: { + // TODO + skipBytes(buffer, length); + break; + } + case ParameterId::PID_PROPERTY_LIST: { + // TODO + skipBytes(buffer, length); + break; + } + case ParameterId::PID_USER_DATA: { + // TODO + skipBytes(buffer, length); + break; + } + case ParameterId::PID_PAD: { + skipBytes(buffer, length); + break; + } + case ParameterId::PID_SENTINEL: { + return true; + } + default: { + // Should not return false for unknown parameters, just skip them, + // otherwise we might miss some important information if the remote + // participant is using some vendor specific parameters that we do not + // know about. + skipBytes(buffer, length); + break; + } + } + // Parameter lists are 4-byte aligned + alignTo4(buffer); + } + return true; +} + +bool ParticipantProxyData::readLocatorIntoList( + CdrReader &buffer, std::array &list) { + int valid_locators = 0; + FullLengthLocator full_length_locator; + for (auto &proxy_locator : list) { + if (!proxy_locator.isValid()) { + bool ret = full_length_locator.readFromBuffer(buffer); + if (ret && full_length_locator.kind == LocatorKind_t::LOCATOR_KIND_UDPv4) { + proxy_locator = LocatorIPv4(full_length_locator); + PPD_LOG("Adding locator: {} {} {} {}", (int)proxy_locator.address[0], + (int)proxy_locator.address[1], (int)proxy_locator.address[2], + (int)proxy_locator.address[3]); + return true; + } else { + PPD_LOG("Ignoring locator: {} {} {} {}", (int)full_length_locator.address[12], + (int)full_length_locator.address[13], (int)full_length_locator.address[14], + (int)full_length_locator.address[15]); + return true; + } + } else { + valid_locators++; + if (valid_locators == Config::SPDP_MAX_NUM_LOCATORS) { + if (buffer.remaining() < sizeof(FullLengthLocator)) { + PPD_LOG("Not enough data left in buffer to read locator"); + return false; + } + skipBytes(buffer, sizeof(FullLengthLocator)); + PPD_LOG("Max number of valid locators exceeded, ignoring this locator as we have at least " + "one valid locator"); + return true; + } + } + } + return false; +} + +#undef PPD_LOG diff --git a/components/rtps_embedded/src/discovery/SEDPAgent.cpp b/components/rtps_embedded/src/discovery/SEDPAgent.cpp new file mode 100644 index 0000000000..28ff79d867 --- /dev/null +++ b/components/rtps_embedded/src/discovery/SEDPAgent.cpp @@ -0,0 +1,475 @@ +/* +The MIT License +Copyright (c) 2019 Lehrstuhl Informatik 11 - RWTH Aachen University +Modifications Copyright (c) 2026 ATDev +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: +The above copyright notice and this permission notice shall be included in +all copies or substantial portions of the Software. +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +THE SOFTWARE + +This file is part of embeddedRTPS. + +Author: i11 - Embedded Software, RWTH Aachen University +*/ + +#include "rtps/discovery/SEDPAgent.hpp" +#include "rtps/discovery/TopicData.hpp" +#include "rtps/entities/Participant.hpp" +#include "rtps/entities/Reader.hpp" +#include "rtps/entities/Writer.hpp" +#include "rtps/messages/MessageTypes.hpp" +#include "rtps/utils/CdrBuffer.hpp" +#include "rtps/utils/Log.hpp" +#include +#include + +using rtps::SEDPAgent; + +#if SEDP_VERBOSE && RTPS_GLOBAL_VERBOSE +#define SEDP_LOG(...) logger_.warn(__VA_ARGS__) +#else +#define SEDP_LOG(...) \ + do { \ + } while (0) +#endif + +SEDPAgent::SEDPAgent() + : espp::BaseComponent("RtpsSEDP", espp::Logger::Verbosity::WARN) {} + +void SEDPAgent::init(Participant &part, const BuiltInEndpoints &endpoints) { + m_part = ∂ + m_endpoints = endpoints; + if (m_endpoints.sedpPubReader != nullptr) { + m_endpoints.sedpPubReader->registerCallback(jumppadPublisherReader, this); + } + if (m_endpoints.sedpSubReader != nullptr) { + m_endpoints.sedpSubReader->registerCallback(jumppadSubscriptionReader, this); + } +} + +void SEDPAgent::registerOnNewPublisherMatchedCallback(void (*callback)(void *arg), void *args) { + mfp_onNewPublisherCallback = callback; + m_onNewPublisherArgs = args; +} + +void SEDPAgent::registerOnNewSubscriberMatchedCallback(void (*callback)(void *arg), void *args) { + mfp_onNewSubscriberCallback = callback; + m_onNewSubscriberArgs = args; +} + +void SEDPAgent::jumppadPublisherReader(void *callee, const ReaderCacheChange &cacheChange) { + auto agent = static_cast(callee); + agent->handlePublisherReaderMessage(cacheChange); +} + +void SEDPAgent::jumppadSubscriptionReader(void *callee, const ReaderCacheChange &cacheChange) { + auto agent = static_cast(callee); + agent->handleSubscriptionReaderMessage(cacheChange); +} + +void SEDPAgent::handlePublisherReaderMessage(const ReaderCacheChange &change) { + std::lock_guard lock(m_mutex); +#if SEDP_VERBOSE + SEDP_LOG("New publisher"); +#endif + + if (!change.copyInto(m_buffer, sizeof(m_buffer) / sizeof(m_buffer[0]))) { +#if SEDP_VERBOSE + SEDP_LOG("EDPAgent: Buffer too small."); +#endif + return; + } + + TopicData topicData; + if (topicData.readFromBuffer(std::span(m_buffer, change.getDataSize()))) { + handlePublisherReaderMessage(topicData, change); + } +} + +void SEDPAgent::addUnmatchedRemoteWriter(const TopicData &writerData) { + addUnmatchedRemoteWriter(TopicDataCompressed(writerData)); +} + +void SEDPAgent::addUnmatchedRemoteReader(const TopicData &readerData) { + addUnmatchedRemoteReader(TopicDataCompressed(readerData)); +} + +void SEDPAgent::addUnmatchedRemoteWriter(const TopicDataCompressed &writerData) { + if (m_unmatchedRemoteWriters.isFull()) { +#if SEDP_VERBOSE + SEDP_LOG("List of unmatched remote writers is full."); +#endif + return; + } + SEDP_LOG("Adding unmatched remote writer {:x} {:x}.", writerData.topicHash, writerData.typeHash); + m_unmatchedRemoteWriters.add(writerData); +} + +void SEDPAgent::addUnmatchedRemoteReader(const TopicDataCompressed &readerData) { + if (m_unmatchedRemoteReaders.isFull()) { +#if SEDP_VERBOSE + SEDP_LOG("List of unmatched remote readers is full."); +#endif + return; + } + SEDP_LOG("Adding unmatched remote reader {:x} {:x}.", readerData.topicHash, readerData.typeHash); + m_unmatchedRemoteReaders.add(readerData); +} + +void SEDPAgent::removeUnmatchedEntity(const Guid_t &guid) { + auto isElementToRemove = [&](const TopicDataCompressed &topicData) { + return topicData.endpointGuid == guid; + }; + + auto thunk = [](void *arg, const TopicDataCompressed &value) { + return (*static_cast(arg))(value); + }; + + m_unmatchedRemoteReaders.remove(thunk, &isElementToRemove); + m_unmatchedRemoteWriters.remove(thunk, &isElementToRemove); +} + +void SEDPAgent::removeUnmatchedEntitiesOfParticipant(const GuidPrefix_t &guidPrefix) { + std::lock_guard lock(m_mutex); + auto isElementToRemove = [&](const TopicDataCompressed &topicData) { + return topicData.endpointGuid.prefix == guidPrefix; + }; + + auto thunk = [](void *arg, const TopicDataCompressed &value) { + return (*static_cast(arg))(value); + }; + + m_unmatchedRemoteReaders.remove(thunk, &isElementToRemove); + m_unmatchedRemoteWriters.remove(thunk, &isElementToRemove); +} + +uint32_t SEDPAgent::getNumRemoteUnmatchedReaders() { + return m_unmatchedRemoteReaders.getNumElements(); +} + +uint32_t SEDPAgent::getNumRemoteUnmatchedWriters() { + return m_unmatchedRemoteWriters.getNumElements(); +} + +void SEDPAgent::handlePublisherReaderMessage(const TopicData &writerData, + const ReaderCacheChange &change) { + // TODO Is it okay to add Endpoint if the respective participant is unknown + // participant? + if (!m_part->findRemoteParticipant(writerData.endpointGuid.prefix)) { + return; + } + + if (writerData.isDisposedFlagSet() || writerData.isUnregisteredFlagSet()) { + handleRemoteEndpointDeletion(writerData, change); + return; + } + +#if SEDP_VERBOSE + SEDP_LOG("PUB T/D {}/{}", writerData.topicName, writerData.typeName); +#endif + Reader *reader = m_part->getMatchingReader(writerData); + if (reader == nullptr) { +#if SEDP_VERBOSE + SEDP_LOG("SEDPAgent: Couldn't find reader for new Publisher[{}, {}]", writerData.topicName, + writerData.typeName); +#endif + addUnmatchedRemoteWriter(writerData); + return; + } + // TODO check policies +#if SEDP_VERBOSE + if (writerData.reliabilityKind == ReliabilityKind_t::RELIABLE) { + SEDP_LOG("Found a new reliable publisher"); + } else { + SEDP_LOG("Found a new best-effort publisher"); + } +#endif + reader->addNewMatchedWriter( + WriterProxy{writerData.endpointGuid, LocatorIPv4(writerData.unicastLocator), + (writerData.reliabilityKind == ReliabilityKind_t::RELIABLE)}); + if (mfp_onNewPublisherCallback != nullptr) { + mfp_onNewPublisherCallback(m_onNewPublisherArgs); + } +} + +void SEDPAgent::handleSubscriptionReaderMessage(const ReaderCacheChange &change) { + std::lock_guard lock(m_mutex); +#if SEDP_VERBOSE + SEDP_LOG("New subscriber"); +#endif + + if (!change.copyInto(m_buffer, sizeof(m_buffer) / sizeof(m_buffer[0]))) { +#if SEDP_VERBOSE + SEDP_LOG("SEDPAgent: Buffer too small."); +#endif + return; + } + + TopicData topicData; + if (topicData.readFromBuffer(std::span(m_buffer, change.getDataSize()))) { + handleSubscriptionReaderMessage(topicData, change); + } +} + +void SEDPAgent::handleRemoteEndpointDeletion(const TopicData &topic, + const ReaderCacheChange &change) { + SEDP_LOG("Endpoint deletion message SN {}.{} GUID {} {} {} {}", (int)change.sn.high, + (int)change.sn.low, change.writerGuid.prefix.id[0], change.writerGuid.prefix.id[1], + change.writerGuid.prefix.id[2], change.writerGuid.prefix.id[3]); + if (!topic.entityIdFromKeyHashValid) { + return; + } + + Guid_t guid; + guid.prefix = topic.endpointGuid.prefix; + guid.entityId = topic.entityIdFromKeyHash; + + // Remove entity ID from all proxies of local endpoints + m_part->removeProxyFromAllEndpoints(guid); + + // Remove entity ID from unmatched endpoints + removeUnmatchedEntity(guid); +} + +void SEDPAgent::handleSubscriptionReaderMessage(const TopicData &readerData, + const ReaderCacheChange &change) { + if (!m_part->findRemoteParticipant(readerData.endpointGuid.prefix)) { + return; + } + + if (readerData.isDisposedFlagSet() || readerData.isUnregisteredFlagSet()) { + handleRemoteEndpointDeletion(readerData, change); + return; + } + + Writer *writer = m_part->getMatchingWriter(readerData); +#if SEDP_VERBOSE + SEDP_LOG("SUB T/D {}/{}", readerData.topicName, readerData.typeName); +#endif + if (writer == nullptr) { +#if SEDP_VERBOSE + SEDP_LOG("SEDPAgent: Couldn't find writer for new subscriber[{}, {}]", readerData.topicName, + readerData.typeName); +#endif + addUnmatchedRemoteReader(readerData); + return; + } + + // TODO check policies +#if SEDP_VERBOSE + if (readerData.reliabilityKind == ReliabilityKind_t::RELIABLE) { + SEDP_LOG("Found a new reliable subscriber"); + } else { + SEDP_LOG("Found a new best-effort subscriber"); + } +#endif + if (readerData.multicastLocator.kind == rtps::LocatorKind_t::LOCATOR_KIND_UDPv4) { + writer->addNewMatchedReader( + ReaderProxy{readerData.endpointGuid, LocatorIPv4(readerData.unicastLocator), + LocatorIPv4(readerData.multicastLocator), + (readerData.reliabilityKind == ReliabilityKind_t::RELIABLE)}); + } else { + writer->addNewMatchedReader( + ReaderProxy{readerData.endpointGuid, LocatorIPv4(readerData.unicastLocator), + (readerData.reliabilityKind == ReliabilityKind_t::RELIABLE)}); + } + + if (mfp_onNewSubscriberCallback != nullptr) { + mfp_onNewSubscriberCallback(m_onNewSubscriberArgs); + } +} + +void SEDPAgent::tryMatchUnmatchedEndpoints() { + // Try to match remote readers with local writers + for (auto &proxy : m_unmatchedRemoteReaders) { + auto writer = m_part->getMatchingWriter(proxy); + if (writer != nullptr) { + writer->addNewMatchedReader(ReaderProxy{proxy.endpointGuid, proxy.unicastLocator, + proxy.multicastLocator, proxy.is_reliable}); + removeUnmatchedEntity(proxy.endpointGuid); + } + } + + // Try to match remote writers with local readers + for (auto &proxy : m_unmatchedRemoteWriters) { + auto reader = m_part->getMatchingReader(proxy); + if (reader != nullptr) { + reader->addNewMatchedWriter( + WriterProxy{proxy.endpointGuid, proxy.unicastLocator, proxy.is_reliable}); + removeUnmatchedEntity(proxy.endpointGuid); + } + } +} + +bool SEDPAgent::addWriter(Writer &writer) { + if (m_endpoints.sedpPubWriter == nullptr) { + return true; + } + EntityKind_t writerKind = writer.m_attributes.endpointGuid.entityId.entityKind; + if (writerKind == EntityKind_t::BUILD_IN_WRITER_WITH_KEY || + writerKind == EntityKind_t::BUILD_IN_WRITER_WITHOUT_KEY) { + return true; // No need to announce builtin endpoints + } + + std::lock_guard lock(m_mutex); + + // Check unmatched writers for this new reader + tryMatchUnmatchedEndpoints(); + + CdrSink sink{asWritableBytes(m_buffer, sizeof(m_buffer) / sizeof(m_buffer[0]))}; + CdrWriter microbuffer(sink); + const uint16_t zero_options = 0; + + writeBytes(microbuffer, rtps::SMElement::SCHEME_PL_CDR_LE.data(), + rtps::SMElement::SCHEME_PL_CDR_LE.size()); + microbuffer.write(zero_options); + writer.m_attributes.serializeInto(microbuffer); + auto change = m_endpoints.sedpPubWriter->newChange(ChangeKind_t::ALIVE, m_buffer, sink.size()); + writer.setSEDPSequenceNumber(change->sequenceNumber); +#if SEDP_VERBOSE + SEDP_LOG("Added new change to sedpPubWriter.\n"); +#endif + return (change != nullptr); +} + +template +bool SEDPAgent::disposeEndpointInSEDPHistory(A *local_endpoint, Writer *sedp_writer) { + return sedp_writer->removeFromHistory(local_endpoint->getSEDPSequenceNumber()); +} + +template +bool SEDPAgent::announceEndpointDeletion(A *local_endpoint, Writer *sedp_endpoint) { + CdrSink sink{asWritableBytes(m_buffer, sizeof(m_buffer) / sizeof(m_buffer[0]))}; + CdrWriter microbuffer(sink); + + microbuffer.write(ParameterId::PID_KEY_HASH); + microbuffer.write(16); + writeBytes(microbuffer, local_endpoint->m_attributes.endpointGuid.prefix.id.data(), + sizeof(GuidPrefix_t::id)); + writeBytes(microbuffer, local_endpoint->m_attributes.endpointGuid.entityId.entityKey.data(), 3); + microbuffer.write( + static_cast(local_endpoint->m_attributes.endpointGuid.entityId.entityKind)); + + microbuffer.write(ParameterId::PID_STATUS_INFO); + microbuffer.write(static_cast(4)); + microbuffer.write(0); + microbuffer.write(0); + microbuffer.write(0); + microbuffer.write(3); + + // Sentinel to terminate inline qos + microbuffer.write(ParameterId::PID_SENTINEL); + microbuffer.write(0); + + // Sentinel to terminate serialized data + microbuffer.write(ParameterId::PID_SENTINEL); + microbuffer.write(0); + + auto ret = sedp_endpoint->newChange(ChangeKind_t::ALIVE, m_buffer, sink.size(), true, true); + SEDP_LOG("Announcing endpoint delete, SN = {}.{}", (int)ret->sequenceNumber.low, + (int)ret->sequenceNumber.high); + return (ret != nullptr); +} + +void SEDPAgent::jumppadTakeProxyOfDisposedReader(const Reader *reader, const WriterProxy &proxy, + void *arg) { + auto agent = static_cast(arg); + TopicDataCompressed topic_data(reader->m_attributes); + topic_data.endpointGuid = proxy.remoteWriterGuid; + topic_data.is_reliable = proxy.is_reliable; + topic_data.multicastLocator.kind = LocatorKind_t::LOCATOR_KIND_INVALID; + topic_data.unicastLocator = proxy.remoteLocator; + agent->addUnmatchedRemoteWriter(topic_data); +} + +void SEDPAgent::jumppadTakeProxyOfDisposedWriter(const Writer *writer, const ReaderProxy &proxy, + void *arg) { + auto agent = static_cast(arg); + TopicDataCompressed topic_data(writer->m_attributes); + topic_data.endpointGuid = proxy.remoteReaderGuid; + topic_data.is_reliable = proxy.is_reliable; + topic_data.multicastLocator.kind = LocatorKind_t::LOCATOR_KIND_INVALID; + topic_data.unicastLocator = proxy.remoteLocator; + agent->addUnmatchedRemoteReader(topic_data); +} + +bool SEDPAgent::deleteReader(Reader *reader) { + std::lock_guard lock(m_mutex); + // Set cache change kind in SEDP endpoint to DISPOSED + if (!disposeEndpointInSEDPHistory(reader, m_endpoints.sedpSubWriter)) { + return false; + } + + // Create Deletion Message [UD] and add to corret builtin endpoint + if (!announceEndpointDeletion(reader, m_endpoints.sedpSubWriter)) { + return false; + } + + // Move all matched proxies of this endpoint to the list of unmatched + // endpoints + reader->dumpAllProxies(SEDPAgent::jumppadTakeProxyOfDisposedReader, this); + + return true; +} + +bool SEDPAgent::deleteWriter(Writer *writer) { + std::lock_guard lock(m_mutex); + // Set cache change kind in SEDP endpoint to DISPOSED + if (!disposeEndpointInSEDPHistory(writer, m_endpoints.sedpPubWriter)) { + return false; + } + + // Create Deletion Mesasge [UD] and add to corret builtin endpoint + if (!announceEndpointDeletion(writer, m_endpoints.sedpPubWriter)) { + return false; + } + + // Move all matched proxies of this endpoint to the list of unmatched + // endpoints + writer->dumpAllProxies(SEDPAgent::jumppadTakeProxyOfDisposedWriter, this); + + return true; +} + +bool SEDPAgent::addReader(Reader &reader) { + if (m_endpoints.sedpSubWriter == nullptr) { + return true; + } + + EntityKind_t readerKind = reader.m_attributes.endpointGuid.entityId.entityKind; + if (readerKind == EntityKind_t::BUILD_IN_READER_WITH_KEY || + readerKind == EntityKind_t::BUILD_IN_READER_WITHOUT_KEY) { + return true; // No need to announce builtin endpoints + } + + std::lock_guard lock(m_mutex); + + // Check unmatched writers for this new reader + tryMatchUnmatchedEndpoints(); + + CdrSink sink{asWritableBytes(m_buffer, sizeof(m_buffer) / sizeof(m_buffer[0]))}; + CdrWriter microbuffer(sink); + const uint16_t zero_options = 0; + + writeBytes(microbuffer, rtps::SMElement::SCHEME_PL_CDR_LE.data(), + rtps::SMElement::SCHEME_PL_CDR_LE.size()); + microbuffer.write(zero_options); + reader.m_attributes.serializeInto(microbuffer); + auto change = m_endpoints.sedpSubWriter->newChange(ChangeKind_t::ALIVE, m_buffer, sink.size()); + reader.setSEDPSequenceNumber(change->sequenceNumber); +#if SEDP_VERBOSE + SEDP_LOG("Added new change to sedpSubWriter.\n"); +#endif + return (change != nullptr); +} diff --git a/components/rtps_embedded/src/discovery/SPDPAgent.cpp b/components/rtps_embedded/src/discovery/SPDPAgent.cpp new file mode 100644 index 0000000000..0f44818b6d --- /dev/null +++ b/components/rtps_embedded/src/discovery/SPDPAgent.cpp @@ -0,0 +1,311 @@ +/* +The MIT License +Copyright (c) 2019 Lehrstuhl Informatik 11 - RWTH Aachen University +Modifications Copyright (c) 2026 ATDev +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: +The above copyright notice and this permission notice shall be included in +all copies or substantial portions of the Software. +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +THE SOFTWARE + +This file is part of embeddedRTPS. + +Author: i11 - Embedded Software, RWTH Aachen University +*/ + +#include "rtps/discovery/SPDPAgent.hpp" +#include "rtps/discovery/ParticipantProxyData.hpp" +#include "rtps/entities/Participant.hpp" +#include "rtps/entities/Reader.hpp" +#include "rtps/entities/Writer.hpp" +#include "rtps/messages/MessageTypes.hpp" +#include "rtps/utils/Log.hpp" +#include "rtps/utils/udpUtils.hpp" + +#include +#include +#include + +using rtps::SPDPAgent; +using rtps::SMElement::BuildInEndpointSet; +using rtps::SMElement::ParameterId; + +SPDPAgent::SPDPAgent() + : espp::BaseComponent("RtpsSPDP", espp::Logger::Verbosity::WARN) {} + +void SPDPAgent::init(Participant &participant, BuiltInEndpoints &endpoints) { + mp_participant = &participant; + m_buildInEndpoints = endpoints; + m_buildInEndpoints.spdpReader->registerCallback(receiveCallback, this); + + addParticipantParameters(); + initialized = true; +} + +void SPDPAgent::start() { m_running = true; } + +void SPDPAgent::stop() { m_running = false; } + +void SPDPAgent::announce() { + // Exactly one announcement cycle; the Domain's protocol scheduler provides + // the SPDP_RESEND_PERIOD_MS cadence (the pacing loop + sleep used to live + // here when this was a dedicated thread body). + const DataSize_t size = static_cast(m_outputSize); + const uint8_t *payload = m_outputBuffer.data(); + // StatelessWriter drops already-sent history; enqueue a fresh SPDP sample + // for each announce cycle. + m_buildInEndpoints.spdpWriter->newChange(ChangeKind_t::ALIVE, payload, size); + if (m_cycleHB == Config::SPDP_CYCLECOUNT_HEARTBEAT) { + m_cycleHB = 0; + mp_participant->checkAndResetHeartbeats(); + } else { + m_cycleHB++; + } +} + +void SPDPAgent::receiveCallback(void *callee, const ReaderCacheChange &cacheChange) { + auto agent = static_cast(callee); + agent->handleSPDPPackage(cacheChange); +} + +void SPDPAgent::handleSPDPPackage(const ReaderCacheChange &cacheChange) { + if (!initialized) { + SPDP_LOG("Callback called without initialization"); + return; + } + + std::lock_guard lock(m_mutex); + if (cacheChange.size > m_inputBuffer.size()) { + SPDP_LOG("Input buffer too small"); + return; + } + + // Something went wrong deserializing remote participant + if (!cacheChange.copyInto(m_inputBuffer.data(), m_inputBuffer.size())) { + return; + } + SPDP_LOG("SPDPPackage size: {}", cacheChange.size); + + if (cacheChange.kind == ChangeKind_t::ALIVE) { + // The payload's endianness is selected by the encapsulation identifier + // (first two bytes); endianness doesn't matter for reading those since + // they are single bytes. + const std::endian endianness = (m_inputBuffer[0] == SMElement::SCHEME_PL_CDR_LE[0] && + m_inputBuffer[1] == SMElement::SCHEME_PL_CDR_LE[1]) + ? std::endian::little + : std::endian::big; + CdrReader buffer(asBytes(m_inputBuffer.data(), m_inputBuffer.size()), endianness); + // Skip the encapsulation identifier and options (2 + 2 bytes) + skipBytes(buffer, 4); + volatile bool success = m_proxyDataBuffer.readFromBuffer(buffer, mp_participant); + if (success) { + // TODO In case we store the history we can free the history mutex here + processProxyData(); + } else { + SPDP_LOG("ParticipantProxyData deserialization failed"); + } + } else { + // TODO RemoveParticipant + } +} + +void SPDPAgent::processProxyData() { + if (m_proxyDataBuffer.m_guid.prefix.id == mp_participant->m_guidPrefix.id) { + return; // Our own packet + } + + SPDP_LOG("Message from GUID = {} {} {} {}", m_proxyDataBuffer.m_guid.prefix.id[4], + m_proxyDataBuffer.m_guid.prefix.id[5], m_proxyDataBuffer.m_guid.prefix.id[6], + m_proxyDataBuffer.m_guid.prefix.id[7]); + const rtps::ParticipantProxyData *remote_part; + remote_part = mp_participant->findRemoteParticipant(m_proxyDataBuffer.m_guid.prefix); + if (remote_part != nullptr) { + SPDP_LOG("Not adding this participant"); + mp_participant->refreshRemoteParticipantLiveliness(m_proxyDataBuffer.m_guid.prefix); + return; // Already in our list + } + + if (mp_participant->addNewRemoteParticipant(m_proxyDataBuffer)) { + addProxiesForBuiltInEndpoints(); + const DataSize_t size = static_cast(m_outputSize); + m_buildInEndpoints.spdpWriter->newChange(ChangeKind_t::ALIVE, m_outputBuffer.data(), size); +#if SPDP_VERBOSE && RTPS_GLOBAL_VERBOSE + SPDP_LOG("Added new participant with guid: "); + printGuidPrefix(m_proxyDataBuffer.m_guid.prefix); + } else { + SPDP_LOG("Failed to add new participant"); + } +#else + } else { + while (1) { + SPDP_LOG("failed to add remote participant"); + } + } +#endif +} + +bool SPDPAgent::addProxiesForBuiltInEndpoints() { + + LocatorIPv4 *locator = nullptr; + + // Check if the remote participants has a locator in our subnet + for (unsigned int i = 0; i < m_proxyDataBuffer.m_metatrafficUnicastLocatorList.size(); i++) { + LocatorIPv4 *l = &(m_proxyDataBuffer.m_metatrafficUnicastLocatorList[i]); + if (l->isValid() && l->isSameSubnet(mp_participant->m_localIpAddress)) { + locator = l; + break; + } + } + + // Fallback: if subnet check fails or local netif is not fully configured yet, + // still use any valid unicast locator so SEDP matching can proceed. + if (!locator) { + for (unsigned int i = 0; i < m_proxyDataBuffer.m_metatrafficUnicastLocatorList.size(); i++) { + LocatorIPv4 *l = &(m_proxyDataBuffer.m_metatrafficUnicastLocatorList[i]); + if (l->isValid()) { + locator = l; + break; + } + } + } + + if (!locator) { + return false; + } + + if (m_proxyDataBuffer.hasPublicationReader()) { + const ReaderProxy proxy{ + {m_proxyDataBuffer.m_guid.prefix, ENTITYID_SEDP_BUILTIN_PUBLICATIONS_READER}, + *locator, + true}; + m_buildInEndpoints.sedpPubWriter->addNewMatchedReader(proxy); + } + + if (m_proxyDataBuffer.hasSubscriptionReader()) { + const ReaderProxy proxy{ + {m_proxyDataBuffer.m_guid.prefix, ENTITYID_SEDP_BUILTIN_SUBSCRIPTIONS_READER}, + *locator, + true}; + m_buildInEndpoints.sedpSubWriter->addNewMatchedReader(proxy); + } + + if (m_proxyDataBuffer.hasPublicationWriter()) { + const WriterProxy proxy{ + {m_proxyDataBuffer.m_guid.prefix, ENTITYID_SEDP_BUILTIN_PUBLICATIONS_WRITER}, + *locator, + true}; + m_buildInEndpoints.sedpPubReader->addNewMatchedWriter(proxy); + m_buildInEndpoints.sedpPubReader->sendPreemptiveAckNack(proxy); + } + + if (m_proxyDataBuffer.hasSubscriptionWriter()) { + const WriterProxy proxy{ + {m_proxyDataBuffer.m_guid.prefix, ENTITYID_SEDP_BUILTIN_SUBSCRIPTIONS_WRITER}, + *locator, + true}; + m_buildInEndpoints.sedpSubReader->addNewMatchedWriter(proxy); + m_buildInEndpoints.sedpSubReader->sendPreemptiveAckNack(proxy); + } + + return true; +} + +void SPDPAgent::addInlineQos(CdrWriter &writer) { + writer.write(ParameterId::PID_KEY_HASH); + writer.write(16); + writeBytes(writer, mp_participant->m_guidPrefix.id.data(), sizeof(GuidPrefix_t::id)); + writeBytes(writer, ENTITYID_BUILD_IN_PARTICIPANT.entityKey.data(), sizeof(EntityId_t::entityKey)); + writer.write(static_cast(ENTITYID_BUILD_IN_PARTICIPANT.entityKind)); + + endCurrentList(writer); +} + +void SPDPAgent::endCurrentList(CdrWriter &writer) { + writer.write(ParameterId::PID_SENTINEL); + writer.write(0); +} + +void SPDPAgent::addParticipantParameters() { + const uint16_t zero_options = 0; + const uint16_t protocolVersionSize = + sizeof(PROTOCOLVERSION.major) + sizeof(PROTOCOLVERSION.minor); + const uint16_t vendorIdSize = Config::VENDOR_ID.vendorId.size(); + const uint16_t locatorSize = sizeof(FullLengthLocator); + const uint16_t durationSize = sizeof(Duration_t::seconds) + sizeof(Duration_t::fraction); + const uint16_t entityKeySize = 3; + const uint16_t entityKindSize = 1; + const uint16_t entityIdSize = entityKeySize + entityKindSize; + const uint16_t guidSize = sizeof(GuidPrefix_t::id) + entityIdSize; + + const FullLengthLocator userUniCastLocator = + getUserUnicastLocator(mp_participant->m_participantId, mp_participant->m_localIpAddress); + const FullLengthLocator builtInUniCastLocator = + getBuiltInUnicastLocator(mp_participant->m_participantId, mp_participant->m_localIpAddress); + const FullLengthLocator builtInMultiCastLocator = getBuiltInMulticastLocator(); + + CdrSink sink{asWritableBytes(m_outputBuffer.data(), m_outputBuffer.size())}; + CdrWriter writer(sink); + + writeBytes(writer, rtps::SMElement::SCHEME_PL_CDR_LE.data(), + rtps::SMElement::SCHEME_PL_CDR_LE.size()); + writer.write(zero_options); + + writer.write(ParameterId::PID_PROTOCOL_VERSION); + writer.write(protocolVersionSize + 2); + writer.write(PROTOCOLVERSION.major); + writer.write(PROTOCOLVERSION.minor); + writer.align(4); // 2 bytes of padding to 4 byte boundary + + writer.write(ParameterId::PID_VENDORID); + writer.write(vendorIdSize + 2); + writeBytes(writer, Config::VENDOR_ID.vendorId.data(), vendorIdSize); + writer.align(4); // 2 bytes of padding to 4 byte boundary + + writer.write(ParameterId::PID_DEFAULT_UNICAST_LOCATOR); + writer.write(locatorSize); + writeBytes(writer, reinterpret_cast(&userUniCastLocator), locatorSize); + + writer.write(ParameterId::PID_METATRAFFIC_UNICAST_LOCATOR); + writer.write(locatorSize); + writeBytes(writer, reinterpret_cast(&builtInUniCastLocator), locatorSize); + + writer.write(ParameterId::PID_METATRAFFIC_MULTICAST_LOCATOR); + writer.write(locatorSize); + writeBytes(writer, reinterpret_cast(&builtInMultiCastLocator), locatorSize); + + writer.write(ParameterId::PID_PARTICIPANT_LEASE_DURATION); + writer.write(durationSize); + writer.write(Config::SPDP_DEFAULT_REMOTE_LEASE_DURATION.seconds); + writer.write(Config::SPDP_DEFAULT_REMOTE_LEASE_DURATION.fraction); + + writer.write(ParameterId::PID_PARTICIPANT_GUID); + writer.write(guidSize); + writeBytes(writer, mp_participant->m_guidPrefix.id.data(), sizeof(GuidPrefix_t::id)); + writeBytes(writer, ENTITYID_BUILD_IN_PARTICIPANT.entityKey.data(), entityKeySize); + writer.write(static_cast(ENTITYID_BUILD_IN_PARTICIPANT.entityKind)); + + writer.write(ParameterId::PID_BUILTIN_ENDPOINT_SET); + writer.write(sizeof(BuildInEndpointSet)); + writer.write(BuildInEndpointSet::DISC_BIE_PARTICIPANT_ANNOUNCER | + BuildInEndpointSet::DISC_BIE_PARTICIPANT_DETECTOR | + BuildInEndpointSet::DISC_BIE_PUBLICATION_ANNOUNCER | + BuildInEndpointSet::DISC_BIE_PUBLICATION_DETECTOR | + BuildInEndpointSet::DISC_BIE_SUBSCRIPTION_ANNOUNCER | + BuildInEndpointSet::DISC_BIE_SUBSCRIPTION_DETECTOR); + + endCurrentList(writer); + + m_outputSize = sink.size(); +} + +#undef SPDP_VERBOSE diff --git a/components/rtps_embedded/src/discovery/TopicData.cpp b/components/rtps_embedded/src/discovery/TopicData.cpp new file mode 100644 index 0000000000..f4d6de3e8b --- /dev/null +++ b/components/rtps_embedded/src/discovery/TopicData.cpp @@ -0,0 +1,286 @@ +/* +The MIT License +Copyright (c) 2019 Lehrstuhl Informatik 11 - RWTH Aachen University +Modifications Copyright (c) 2026 ATDev +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: +The above copyright notice and this permission notice shall be included in +all copies or substantial portions of the Software. +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +THE SOFTWARE + +This file is part of embeddedRTPS. + +Author: i11 - Embedded Software, RWTH Aachen University +*/ +#include "rtps/discovery/TopicData.hpp" +#include "logger.hpp" +#include "rtps/messages/MessageTypes.hpp" +#include +#include + +using rtps::TopicData; +using rtps::TopicDataCompressed; +using rtps::SMElement::ParameterId; + +namespace { +espp::Logger s_topic_data_logger({.tag = "RtpsTopicData", .level = espp::Logger::Verbosity::WARN}); +} + +bool TopicData::isDisposedFlagSet() const { return statusInfoValid && ((statusInfo & 0b1)); } + +bool TopicData::isUnregisteredFlagSet() const { + return statusInfoValid && ((statusInfo & (0b1 << 1)) != 0); +} + +bool TopicData::matchesTopicOf(const TopicData &other) { + return strcmp(this->topicName, other.topicName) == 0 && + strcmp(this->typeName, other.typeName) == 0; +} + +bool TopicData::readFromBuffer(std::span data) { + CdrReader buffer(asBytes(data.data(), data.size())); + + // Reset valid flags, as the respective parameters are optional + statusInfoValid = false; + entityIdFromKeyHashValid = false; + + while (buffer.remaining() >= 4) { + const auto pidRaw = buffer.read(); + const auto lengthRaw = buffer.read(); + if (!pidRaw || !lengthRaw) { + s_topic_data_logger.error("FAILED TO DESERIALIZE TOPIC DATA"); + return false; + } + const auto pid = static_cast(*pidRaw); + const uint16_t length = *lengthRaw; + FullLengthLocator uLoc; + + if (buffer.remaining() < length) { + return false; + } + + switch (pid) { + case ParameterId::PID_ENDPOINT_GUID: { + if (!readBytes(buffer, endpointGuid.prefix.id.data(), endpointGuid.prefix.id.size()) || + !readBytes(buffer, endpointGuid.entityId.entityKey.data(), + endpointGuid.entityId.entityKey.size())) { + s_topic_data_logger.error("FAILED TO DESERIALIZE TOPIC DATA"); + return false; + } + const auto kind = buffer.read(); + if (!kind) { + s_topic_data_logger.error("FAILED TO DESERIALIZE TOPIC DATA"); + return false; + } + endpointGuid.entityId.entityKind = static_cast(*kind); + break; + } + case ParameterId::PID_RELIABILITY: { + const auto kind = buffer.read(); + if (!kind) { + s_topic_data_logger.error("FAILED TO DESERIALIZE TOPIC DATA"); + return false; + } + reliabilityKind = static_cast(*kind); + skipBytes(buffer, 8); + // TODO Skip 8 bytes. don't know what they are yet + break; + } + case ParameterId::PID_SENTINEL: + return true; + case ParameterId::PID_TOPIC_NAME: { + const auto topicNameLength = buffer.read(); + if (!topicNameLength) { + s_topic_data_logger.error("FAILED TO DESERIALIZE TOPIC DATA"); + return false; + } + if (*topicNameLength > Config::MAX_TOPICNAME_LENGTH) { + s_topic_data_logger.warn("Topic name length {} exceeds maximum allowed length {}", + *topicNameLength, Config::MAX_TOPICNAME_LENGTH); + return false; + } + if (!readBytes(buffer, reinterpret_cast(topicName), *topicNameLength)) { + s_topic_data_logger.error("FAILED TO DESERIALIZE TOPIC DATA"); + return false; + } + break; + } + case ParameterId::PID_TYPE_NAME: { + const auto typeNameLength = buffer.read(); + if (!typeNameLength) { + s_topic_data_logger.error("FAILED TO DESERIALIZE TOPIC DATA"); + return false; + } + if (*typeNameLength > Config::MAX_TYPENAME_LENGTH) { + s_topic_data_logger.warn("Type name length {} exceeds maximum allowed length {}", + *typeNameLength, Config::MAX_TYPENAME_LENGTH); + return false; + } + if (!readBytes(buffer, reinterpret_cast(typeName), *typeNameLength)) { + s_topic_data_logger.error("FAILED TO DESERIALIZE TOPIC DATA"); + return false; + } + break; + } + case ParameterId::PID_UNICAST_LOCATOR: + uLoc.readFromBuffer(buffer); + // Accept valid UDPv4 locators even if subnet detection is temporarily + // unavailable (e.g. early startup) to avoid keeping placeholder defaults. + if (uLoc.kind == LocatorKind_t::LOCATOR_KIND_UDPv4) { + unicastLocator = uLoc; + const auto a0 = static_cast(uLoc.address[12]); + const auto a1 = static_cast(uLoc.address[13]); + const auto a2 = static_cast(uLoc.address[14]); + const auto a3 = static_cast(uLoc.address[15]); + const auto port = static_cast(uLoc.port); + s_topic_data_logger.warn("Received unicast locator: {}.{}.{}.{}:{}", a0, a1, a2, a3, port); + } else { + // print warning and the invalid locator for debugging + s_topic_data_logger.warn("Warning: Received invalid unicast locator with kind {}", + static_cast(uLoc.kind)); + } + break; + case ParameterId::PID_MULTICAST_LOCATOR: + multicastLocator.readFromBuffer(buffer); + break; + case ParameterId::PID_STATUS_INFO: { + if (length == 4) { + skipBytes(buffer, 3); // skip first 3 bytes of status info as they are + // reserved parameters + const auto status = buffer.read(); + if (!status) { + s_topic_data_logger.error("FAILED TO DESERIALIZE TOPIC DATA"); + return false; + } + statusInfo = *status; + statusInfoValid = true; + } else { // Ignore Status Info + skipBytes(buffer, length); + } + } break; + case ParameterId::PID_KEY_HASH: // only use case so far is deleting remote + // endpoints + { + if (length == 16) { + if (!readBytes(buffer, endpointGuid.prefix.id.data(), endpointGuid.prefix.id.size()) || + !readBytes(buffer, this->entityIdFromKeyHash.entityKey.data(), + this->entityIdFromKeyHash.entityKey.size())) { + s_topic_data_logger.error("FAILED TO DESERIALIZE TOPIC DATA"); + return false; + } + const auto kind = buffer.read(); + if (!kind) { + s_topic_data_logger.error("FAILED TO DESERIALIZE TOPIC DATA"); + return false; + } + this->entityIdFromKeyHash.entityKind = static_cast(*kind); + entityIdFromKeyHashValid = true; + } else { // Ignore value + skipBytes(buffer, length); + } + } break; + default: + skipBytes(buffer, length); + } + + // Parameter-list elements are 4-byte aligned + alignTo4(buffer); + } + return buffer.remaining() == 0; +} + +bool TopicData::serializeInto(CdrWriter &writer) const { + const uint16_t guidSize = sizeof(GuidPrefix_t::id) + 4; + +#if SUPPRESS_UNICAST + if (multicastLocator.kind != LocatorKind_t::LOCATOR_KIND_UDPv4) { +#endif + writer.write(ParameterId::PID_UNICAST_LOCATOR); + writer.write(sizeof(FullLengthLocator)); + writeBytes(writer, reinterpret_cast(&unicastLocator), + sizeof(FullLengthLocator)); +#if SUPPRESS_UNICAST + } +#endif + + if (multicastLocator.kind == LocatorKind_t::LOCATOR_KIND_UDPv4) { + writer.write(ParameterId::PID_MULTICAST_LOCATOR); + writer.write(sizeof(FullLengthLocator)); + writeBytes(writer, reinterpret_cast(&multicastLocator), + sizeof(FullLengthLocator)); + } + + // It's a 32 bit instead of 16 because it seems like the field is padded. + const auto lenTopicName = static_cast(strlen(topicName) + 1); // + \0 + uint16_t topicAlignment = 0; + if (lenTopicName % 4 != 0) { + topicAlignment = static_cast(4 - (lenTopicName % 4)); + } + const auto totalLengthTopicNameField = + static_cast(sizeof(lenTopicName) + lenTopicName + topicAlignment); + writer.write(ParameterId::PID_TOPIC_NAME); + writer.write(totalLengthTopicNameField); + writer.write(lenTopicName); + writeBytes(writer, reinterpret_cast(topicName), lenTopicName); + writer.align(4); + + // It's a 32 bit instead of 16 because it seems like the field is padded. + const auto lenTypeName = static_cast(strlen(typeName) + 1); // + \0 + uint16_t typeAlignment = 0; + if (lenTypeName % 4 != 0) { + typeAlignment = static_cast(4 - (lenTypeName % 4)); + } + const auto totalLengthTypeNameField = + static_cast(sizeof(lenTypeName) + lenTypeName + typeAlignment); + + writer.write(ParameterId::PID_TYPE_NAME); + writer.write(totalLengthTypeNameField); + writer.write(lenTypeName); + writeBytes(writer, reinterpret_cast(typeName), lenTypeName); + writer.align(4); + + writer.write(ParameterId::PID_KEY_HASH); + writer.write(guidSize); + writeBytes(writer, endpointGuid.prefix.id.data(), endpointGuid.prefix.id.size()); + writeBytes(writer, endpointGuid.entityId.entityKey.data(), + endpointGuid.entityId.entityKey.size()); + writer.write(static_cast(endpointGuid.entityId.entityKind)); + + writer.write(ParameterId::PID_ENDPOINT_GUID); + writer.write(guidSize); + writeBytes(writer, endpointGuid.prefix.id.data(), endpointGuid.prefix.id.size()); + writeBytes(writer, endpointGuid.entityId.entityKey.data(), + endpointGuid.entityId.entityKey.size()); + writer.write(static_cast(endpointGuid.entityId.entityKind)); + + const uint8_t unidentifiedOffset = 8; + writer.write(ParameterId::PID_RELIABILITY); + writer.write(sizeof(ReliabilityKind_t) + unidentifiedOffset); + writer.write(static_cast(reliabilityKind)); + writer.write(0); // unidentified additional value + writer.write(0); // unidentified additional value + + writer.write(ParameterId::PID_DURABILITY); + writer.write(sizeof(DurabilityKind_t)); + writer.write(static_cast(durabilityKind)); + + writer.write(ParameterId::PID_SENTINEL); + writer.write(0); + + return writer.ok(); +} + +bool TopicDataCompressed::matchesTopicOf(const TopicData &other) const { + return (hashCharArray(other.topicName, sizeof(other.topicName)) == topicHash && + hashCharArray(other.typeName, sizeof(other.typeName)) == typeHash); +} diff --git a/components/rtps_embedded/src/entities/Domain.cpp b/components/rtps_embedded/src/entities/Domain.cpp new file mode 100644 index 0000000000..45ac57cd8d --- /dev/null +++ b/components/rtps_embedded/src/entities/Domain.cpp @@ -0,0 +1,683 @@ +/* +The MIT License +Copyright (c) 2019 Lehrstuhl Informatik 11 - RWTH Aachen University +Modifications Copyright (c) 2026 ATDev +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: +The above copyright notice and this permission notice shall be included in +all copies or substantial portions of the Software. +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +THE SOFTWARE + +This file is part of embeddedRTPS. + +Author: i11 - Embedded Software, RWTH Aachen University +*/ + +#include "rtps/entities/Domain.hpp" +#include "rtps/utils/Log.hpp" +#include "rtps/utils/udpUtils.hpp" +#include +#include +#include + +#if defined(ESP_PLATFORM) +#include "esp_mac.h" +#endif + +#if DOMAIN_VERBOSE && RTPS_GLOBAL_VERBOSE +#define DOMAIN_LOG(...) logger_.warn(__VA_ARGS__) +#else +#define DOMAIN_LOG(...) \ + do { \ + } while (0) +#endif + +using rtps::Domain; + +Domain::Domain(const rtps::Ip4AddressBytes &localIpAddress) + : espp::BaseComponent("RtpsDomain", espp::Logger::Verbosity::WARN) + , m_defaultTransport(&Domain::datagramJumppad, this) + , m_transport(&m_defaultTransport) + , m_localIpAddress(localIpAddress) { + m_transportSetupOk = initializeTransport(); +} + +Domain::Domain(rtps::EsppTransport &transport, const rtps::Ip4AddressBytes &localIpAddress) + : espp::BaseComponent("RtpsDomain", espp::Logger::Verbosity::WARN) + , m_defaultTransport(&Domain::datagramJumppad, this) + , m_transport(&transport) + , m_localIpAddress(localIpAddress) { + m_transportSetupOk = initializeTransport(); +} + +bool Domain::initializeTransport() { + assert(m_transport != nullptr); + bool success = true; + success = + m_transport->ensureReceivePort(getUserMulticastPort(), /*is_multicast=*/true) && success; + success = + m_transport->ensureReceivePort(getBuiltInMulticastPort(), /*is_multicast=*/true) && success; + success = m_transport->joinMultiCastGroup({239, 255, 0, 1}) && success; + return success; +} + +Domain::~Domain() { stop(); } + +bool Domain::completeInit() { + if (!m_transportSetupOk) { + DOMAIN_LOG("Failed transport setup. Domain initialization aborted."); + m_initComplete = false; + return false; + } + + m_initComplete = true; + + // Start the protocol scheduler: one task drives every participant's SPDP + // announcements and every stateful writer's heartbeat cadence. + m_nextSpdpAnnounce = std::chrono::steady_clock::now(); + for (auto &writer : m_statefulWriters) { + writer.setProtocolNudge([this]() { nudgeProtocol(); }); + } + espp::Task::Config task_config; + task_config.callback = [this](std::mutex &m, std::condition_variable &cv, bool ¬ified) { + return protocolLoop(m, cv, notified); + }; + task_config.task_config.name = "rtps_protocol"; + task_config.task_config.stack_size_bytes = + Config::SPDP_WRITER_STACKSIZE > 4096 ? Config::SPDP_WRITER_STACKSIZE : 4096; + task_config.task_config.priority = Config::SPDP_WRITER_PRIO; + task_config.log_level = espp::Logger::Verbosity::WARN; + m_protocolTask = espp::Task::make_unique(task_config); + (void)m_protocolTask->start(); + + for (uint8_t slot = 0; slot < m_numParticipants; ++slot) { + m_participants[slot].getSPDPAgent().start(); + } + return m_initComplete; +} + +void Domain::stop() { + if (m_protocolTask) { + m_protocolStopRequested = true; + nudgeProtocol(); // wake the loop so it observes the stop flag + m_protocolTask->stop(); // returns promptly + m_protocolTask.reset(); + m_protocolStopRequested = false; + } + for (uint8_t slot = 0; slot < m_numParticipants; ++slot) { + m_participants[slot].getSPDPAgent().stop(); + } + // Stop receive dispatch + the worker pool BEFORE participants/writers are + // torn down: queued jobs and in-flight datagram handlers reference them. + m_transport->stop(); +} + +void Domain::receiveJumppad(void *callee, const PacketInfo &packet) { + auto domain = static_cast(callee); + domain->receiveCallback(packet); +} + +bool Domain::protocolLoop(std::mutex &m, std::condition_variable &cv, bool ¬ified) { + using clock = std::chrono::steady_clock; + const auto now = clock::now(); + + // SPDP announcements for every running participant, at the configured cadence. + if (now >= m_nextSpdpAnnounce) { + for (uint8_t slot = 0; slot < m_numParticipants; ++slot) { + auto &agent = m_participants[slot].getSPDPAgent(); + if (agent.isRunning()) { + agent.announce(); + } + } + m_nextSpdpAnnounce = now + std::chrono::milliseconds(Config::SPDP_RESEND_PERIOD_MS); + } + + // Heartbeat ticks for every (initialized) stateful writer; each returns its + // next deadline. Uninitialized writers report a far-future deadline. + auto next_deadline = m_nextSpdpAnnounce; + for (auto &writer : m_statefulWriters) { + const auto writer_deadline = writer.heartbeatTick(now); + next_deadline = std::min(next_deadline, writer_deadline); + } + + // Sleep until the earliest deadline; a publish on a reliable writer (or + // stop()) notifies the cv to re-evaluate immediately. + std::unique_lock lock(m); + m_protocolMutex = &m; + m_protocolCv = &cv; + m_protocolNotified = ¬ified; + cv.wait_until(lock, next_deadline, [¬ified] { return notified; }); + if (notified) { + notified = false; + // Both Domain::stop() and a heartbeat nudge arrive via this cv; the + // explicit flag (set before the stop notification) disambiguates. A nudge + // simply re-evaluates deadlines on the next iteration. + if (m_protocolStopRequested.load()) { + return true; // stop requested + } + } + return m_protocolStopRequested.load(); // keep running unless stopping +} + +void Domain::nudgeProtocol() { + if (m_protocolMutex != nullptr && m_protocolCv != nullptr && m_protocolNotified != nullptr) { + std::lock_guard lock(*m_protocolMutex); + *m_protocolNotified = true; + m_protocolCv->notify_all(); + } +} + +void Domain::datagramJumppad(void *arg, const uint8_t *data, std::size_t size, Ip4Port_t localPort, + Ip4Port_t remotePort, const Ip4AddressBytes &remoteAddress) { + auto *domain = static_cast(arg); + + PacketInfo packet; + packet.destAddr = remoteAddress; + packet.destPort = localPort; + packet.srcPort = remotePort; + if (size > 0 && data != nullptr) { + packet.payload.assign(data, data + size); + } + // Process inline on the transport worker that delivered the datagram: the + // reactor's one-shot arming already serializes per socket, replacing the + // former queue + reader-worker indirection. + domain->receiveCallback(packet); +} + +void Domain::receiveCallback(const PacketInfo &packet) { + if (packet.payload.empty()) { + DOMAIN_LOG("Dropping packet without payload"); + return; + } + + const uint8_t *payload = packet.payload.data(); + DataSize_t payload_size = static_cast(packet.payload.size()); + + if (isMetaMultiCastPort(packet.destPort)) { + // Pass to all + DOMAIN_LOG("Domain: Multicast to port {}", packet.destPort); + for (uint8_t slot = 0; slot < m_numParticipants; ++slot) { + m_participants[slot].newMessage(payload, payload_size); + } + // First Check if UserTraffic Multicast + } else if (isUserMultiCastPort(packet.destPort)) { + // Pass to Participant with assigned Multicast Adress (Port ist everytime + // the same) + DOMAIN_LOG("Domain: Got user multicast message on port {}", packet.destPort); + for (uint8_t slot = 0; slot < m_numParticipants; ++slot) { + if (m_participants[slot].hasReaderWithMulticastLocator(packet.destAddr)) { + DOMAIN_LOG("Domain: Forward Multicast only to Participant: {}", slot); + m_participants[slot].newMessage(payload, payload_size); + } + } + } else { + // Pass to addressed one only (Unicast, by Port) + ParticipantId_t id = + getParticipantIdFromUnicastPort(packet.destPort, isUserPort(packet.destPort)); + if (id != PARTICIPANT_ID_INVALID) { + DOMAIN_LOG("Domain: Got unicast message on port {}", packet.destPort); + // Ids may be non-contiguous after port probing, so look the participant + // up by id rather than indexing slots arithmetically. + Participant *target = findParticipantById(id); + if (target != nullptr) { + target->newMessage(payload, payload_size); + } else { + DOMAIN_LOG("Domain: No local participant with id {}.", id); + } + } else { + DOMAIN_LOG("Domain: Got message to port {}: no matching participant", packet.destPort); + } + } +} + +rtps::Participant *Domain::createParticipant() { + + DOMAIN_LOG("Domain: Creating new participant."); + + if (m_initComplete || m_participants.size() <= m_numParticipants) { + return nullptr; + } + + // Probe for a participant id whose unicast ports are free on this host. + // Unicast channels bind with reuse disabled, so an id already used by + // another process fails loudly here and we advance to the next id - the + // same strategy FastDDS uses. Ids may therefore skip values; slots are + // tracked separately (m_numParticipants). + ParticipantId_t candidate = m_nextParticipantId; + const ParticipantId_t last_candidate = m_nextParticipantId + PARTICIPANT_PORT_PROBE_LIMIT; + bool ports_ok = false; + for (; candidate < last_candidate; ++candidate) { + if (!m_transport->ensureReceivePort(getUserUnicastPort(candidate), /*is_multicast=*/false)) { + continue; + } + if (m_transport->ensureReceivePort(getBuiltInUnicastPort(candidate), /*is_multicast=*/false)) { + ports_ok = true; + break; + } + // unwind the half-registered probe before trying the next id + m_transport->releaseReceivePort(getUserUnicastPort(candidate)); + } + if (!ports_ok) { + DOMAIN_LOG("No free unicast ports for a new participant (probed {} ids from {})", + PARTICIPANT_PORT_PROBE_LIMIT, m_nextParticipantId); + m_transportSetupOk = false; + return nullptr; + } + + auto &entry = m_participants[m_numParticipants]; + ++m_numParticipants; + entry.reuse(generateGuidPrefix(candidate), candidate, m_localIpAddress); + createBuiltinWritersAndReaders(entry); + m_nextParticipantId = static_cast(candidate + 1); + return &entry; +} + +void Domain::createBuiltinWritersAndReaders(Participant &part) { + // SPDP + StatelessWriter *spdpWriter = + getNextUnusedEndpoint(m_statelessWriters); + StatelessReader *spdpReader = + getNextUnusedEndpoint(m_statelessReaders); + + TopicData spdpWriterAttributes; + spdpWriterAttributes.topicName[0] = '\0'; + spdpWriterAttributes.typeName[0] = '\0'; + spdpWriterAttributes.reliabilityKind = ReliabilityKind_t::BEST_EFFORT; + spdpWriterAttributes.durabilityKind = DurabilityKind_t::TRANSIENT_LOCAL; + spdpWriterAttributes.endpointGuid.prefix = part.m_guidPrefix; + spdpWriterAttributes.endpointGuid.entityId = ENTITYID_SPDP_BUILTIN_PARTICIPANT_WRITER; + spdpWriterAttributes.unicastLocator = getBuiltInMulticastLocator(); + + spdpWriter->init(spdpWriterAttributes, TopicKind_t::WITH_KEY, *m_transport); + spdpWriter->addNewMatchedReader( + ReaderProxy{{part.m_guidPrefix, ENTITYID_SPDP_BUILTIN_PARTICIPANT_READER}, + LocatorIPv4(getBuiltInMulticastLocator()), + false}); + + TopicData spdpReaderAttributes; + spdpReaderAttributes.endpointGuid = {part.m_guidPrefix, ENTITYID_SPDP_BUILTIN_PARTICIPANT_READER}; + spdpReader->init(spdpReaderAttributes); + + // SEDP + + // Prepare attributes + TopicData sedpAttributes; + sedpAttributes.topicName[0] = '\0'; + sedpAttributes.typeName[0] = '\0'; + sedpAttributes.reliabilityKind = ReliabilityKind_t::RELIABLE; + sedpAttributes.durabilityKind = DurabilityKind_t::TRANSIENT_LOCAL; + sedpAttributes.endpointGuid.prefix = part.m_guidPrefix; + sedpAttributes.unicastLocator = getBuiltInUnicastLocator(part.m_participantId, m_localIpAddress); + + // READER + StatefulReader *sedpPubReader = + getNextUnusedEndpoint(m_statefulReaders); + sedpAttributes.endpointGuid.entityId = ENTITYID_SEDP_BUILTIN_PUBLICATIONS_READER; + sedpPubReader->init(sedpAttributes, *m_transport); + + StatefulReader *sedpSubReader = + getNextUnusedEndpoint(m_statefulReaders); + sedpAttributes.endpointGuid.entityId = ENTITYID_SEDP_BUILTIN_SUBSCRIPTIONS_READER; + sedpSubReader->init(sedpAttributes, *m_transport); + + // WRITER + StatefulWriter *sedpPubWriter = + getNextUnusedEndpoint(m_statefulWriters); + sedpAttributes.endpointGuid.entityId = ENTITYID_SEDP_BUILTIN_PUBLICATIONS_WRITER; + sedpPubWriter->init(sedpAttributes, TopicKind_t::NO_KEY, *m_transport); + + StatefulWriter *sedpSubWriter = + getNextUnusedEndpoint(m_statefulWriters); + sedpAttributes.endpointGuid.entityId = ENTITYID_SEDP_BUILTIN_SUBSCRIPTIONS_WRITER; + sedpSubWriter->init(sedpAttributes, TopicKind_t::NO_KEY, *m_transport); + + // COLLECT + BuiltInEndpoints endpoints{}; + endpoints.spdpWriter = spdpWriter; + endpoints.spdpReader = spdpReader; + endpoints.sedpPubReader = sedpPubReader; + endpoints.sedpSubReader = sedpSubReader; + endpoints.sedpPubWriter = sedpPubWriter; + endpoints.sedpSubWriter = sedpSubWriter; + + part.addBuiltInEndpoints(endpoints); +} + +rtps::Participant *Domain::findParticipantById(ParticipantId_t id) { + for (uint8_t slot = 0; slot < m_numParticipants; ++slot) { + if (m_participants[slot].m_participantId == id) { + return &m_participants[slot]; + } + } + return nullptr; +} + +void Domain::registerMulticastPort(FullLengthLocator mcastLocator) { + if (mcastLocator.kind == LocatorKind_t::LOCATOR_KIND_UDPv4) { + m_transportSetupOk = + m_transport->ensureReceivePort(mcastLocator.getLocatorPort(), /*is_multicast=*/true) && + m_transportSetupOk; + } +} + +rtps::Reader *Domain::readerExists(Participant &part, const char *topicName, const char *typeName, + bool reliable) { + std::lock_guard lock(m_mutex); + if (reliable) { + for (unsigned int i = 0; i < m_statefulReaders.size(); i++) { + if (m_statefulReaders[i].isInitialized()) { + if (strncmp(m_statefulReaders[i].m_attributes.topicName, topicName, + Config::MAX_TYPENAME_LENGTH) != 0) { + continue; + } + + if (strncmp(m_statefulReaders[i].m_attributes.typeName, typeName, + Config::MAX_TYPENAME_LENGTH) != 0) { + continue; + } + + DOMAIN_LOG("StatefulReader exists already [{}, {}]", topicName, typeName); + + return &m_statefulReaders[i]; + } + } + } else { + for (unsigned int i = 0; i < m_statelessReaders.size(); i++) { + if (m_statelessReaders[i].isInitialized()) { + if (strncmp(m_statelessReaders[i].m_attributes.topicName, topicName, + Config::MAX_TYPENAME_LENGTH) != 0) { + continue; + } + + if (strncmp(m_statelessReaders[i].m_attributes.typeName, typeName, + Config::MAX_TYPENAME_LENGTH) != 0) { + continue; + } + + DOMAIN_LOG("StatelessReader exists [{}, {}]", topicName, typeName); + + return &m_statelessReaders[i]; + } + } + } + + return nullptr; +} + +rtps::Writer *Domain::writerExists(Participant &part, const char *topicName, const char *typeName, + bool reliable) { + std::lock_guard lock(m_mutex); + if (reliable) { + for (unsigned int i = 0; i < m_statefulWriters.size(); i++) { + if (m_statefulWriters[i].isInitialized()) { + if (strncmp(m_statefulWriters[i].m_attributes.topicName, topicName, + Config::MAX_TYPENAME_LENGTH) != 0) { + continue; + } + + if (strncmp(m_statefulWriters[i].m_attributes.typeName, typeName, + Config::MAX_TYPENAME_LENGTH) != 0) { + continue; + } + + DOMAIN_LOG("StatefulWriter exists [{}, {}]", topicName, typeName); + + return &m_statefulWriters[i]; + } + } + } else { + for (unsigned int i = 0; i < m_statelessWriters.size(); i++) { + if (m_statelessWriters[i].isInitialized()) { + if (strncmp(m_statelessWriters[i].m_attributes.topicName, topicName, + Config::MAX_TYPENAME_LENGTH) != 0) { + continue; + } + + if (strncmp(m_statelessWriters[i].m_attributes.typeName, typeName, + Config::MAX_TYPENAME_LENGTH) != 0) { + continue; + } + + DOMAIN_LOG("StatelessWriter exists [{}, {}]", topicName, typeName); + + return &m_statelessWriters[i]; + } + } + } + + return nullptr; +} + +rtps::Writer *Domain::createWriter(Participant &part, const char *topicName, const char *typeName, + bool reliable, bool enforceUnicast) { + std::lock_guard lock(m_mutex); + StatelessWriter *statelessWriter = + getNextUnusedEndpoint(m_statelessWriters); + StatefulWriter *statefulWriter = + getNextUnusedEndpoint(m_statefulWriters); + + // Check if there is enough capacity for more writers + if ((reliable && statefulWriter == nullptr) || (!reliable && statelessWriter == nullptr) || + part.isWritersFull()) { + + DOMAIN_LOG("No Writer created. Max Number of Writers reached."); + + return nullptr; + } + + // TODO Distinguish WithKey and NoKey (Also changes EntityKind) + TopicData attributes; + + if (strlen(topicName) >= Config::MAX_TOPICNAME_LENGTH || + strlen(typeName) >= Config::MAX_TYPENAME_LENGTH) { + return nullptr; + } + strncpy(attributes.topicName, topicName, Config::MAX_TOPICNAME_LENGTH); + strncpy(attributes.typeName, typeName, Config::MAX_TYPENAME_LENGTH); + attributes.topicName[Config::MAX_TOPICNAME_LENGTH - 1] = '\0'; + attributes.typeName[Config::MAX_TYPENAME_LENGTH - 1] = '\0'; + attributes.endpointGuid.prefix = part.m_guidPrefix; + attributes.endpointGuid.entityId = {part.getNextUserEntityKey(), + EntityKind_t::USER_DEFINED_WRITER_WITHOUT_KEY}; + attributes.unicastLocator = getUserUnicastLocator(part.m_participantId, m_localIpAddress); + attributes.durabilityKind = DurabilityKind_t::TRANSIENT_LOCAL; + + DOMAIN_LOG("Creating writer[{}, {}]", topicName, typeName); + + if (reliable) { + attributes.reliabilityKind = ReliabilityKind_t::RELIABLE; + + if (!statefulWriter->init(attributes, TopicKind_t::NO_KEY, *m_transport, enforceUnicast)) { + DOMAIN_LOG("StatefulWriter init failed."); + return nullptr; + } + + if (!part.addWriter(statefulWriter)) { + return nullptr; + } + return statefulWriter; + } else { + attributes.reliabilityKind = ReliabilityKind_t::BEST_EFFORT; + + if (!statelessWriter->init(attributes, TopicKind_t::NO_KEY, *m_transport, enforceUnicast)) { + DOMAIN_LOG("StatelessWriter init failed."); + return nullptr; + } + + if (!part.addWriter(statelessWriter)) { + return nullptr; + } + return statelessWriter; + } +} + +rtps::Reader *Domain::createReader(Participant &part, const char *topicName, const char *typeName, + bool reliable, rtps::Ip4AddressBytes mcastaddress) { + std::lock_guard lock(m_mutex); + StatelessReader *statelessReader = + getNextUnusedEndpoint(m_statelessReaders); + StatefulReader *statefulReader = + getNextUnusedEndpoint(m_statefulReaders); + + if ((reliable && statefulReader == nullptr) || (!reliable && statelessReader == nullptr) || + part.isReadersFull()) { + + DOMAIN_LOG("No Reader created. Max Number of Readers reached."); + + return nullptr; + } + + // TODO Distinguish WithKey and NoKey (Also changes EntityKind) + TopicData attributes; + + if (strlen(topicName) >= Config::MAX_TOPICNAME_LENGTH || + strlen(typeName) >= Config::MAX_TYPENAME_LENGTH) { + return nullptr; + } + strncpy(attributes.topicName, topicName, Config::MAX_TOPICNAME_LENGTH); + strncpy(attributes.typeName, typeName, Config::MAX_TYPENAME_LENGTH); + attributes.topicName[Config::MAX_TOPICNAME_LENGTH - 1] = '\0'; + attributes.typeName[Config::MAX_TYPENAME_LENGTH - 1] = '\0'; + attributes.endpointGuid.prefix = part.m_guidPrefix; + attributes.endpointGuid.entityId = {part.getNextUserEntityKey(), + EntityKind_t::USER_DEFINED_READER_WITHOUT_KEY}; + attributes.unicastLocator = getUserUnicastLocator(part.m_participantId, m_localIpAddress); + if (!isZeroAddress(mcastaddress)) { + if (isMulticastAddress(mcastaddress)) { + attributes.multicastLocator = rtps::FullLengthLocator::createUDPv4Locator( + mcastaddress[0], mcastaddress[1], mcastaddress[2], mcastaddress[3], + getUserMulticastPort()); + m_transportSetupOk = + m_transport->joinMultiCastGroup( + {attributes.multicastLocator.address[12], attributes.multicastLocator.address[13], + attributes.multicastLocator.address[14], attributes.multicastLocator.address[15]}) && + m_transportSetupOk; + registerMulticastPort(attributes.multicastLocator); + + DOMAIN_LOG("Multicast enabled!"); + + } else { + + DOMAIN_LOG("This is not a Multicastaddress!"); + } + } + attributes.durabilityKind = DurabilityKind_t::VOLATILE; + + DOMAIN_LOG("Creating reader[{}, {}]", topicName, typeName); + + if (reliable) { + + attributes.reliabilityKind = ReliabilityKind_t::RELIABLE; + + statefulReader->init(attributes, *m_transport); + + if (!part.addReader(statefulReader)) { + DOMAIN_LOG("Failed to add reader to participant."); + + return nullptr; + } + return statefulReader; + } else { + + attributes.reliabilityKind = ReliabilityKind_t::BEST_EFFORT; + + statelessReader->init(attributes); + + if (!part.addReader(statelessReader)) { + return nullptr; + } + return statelessReader; + } +} + +bool rtps::Domain::deleteReader(Participant &part, Reader *reader) { + std::lock_guard lock(m_mutex); + if (reader == nullptr || !reader->isInitialized()) { + return false; + } + if (!part.deleteReader(reader)) { + return false; + } + + reader->reset(); + return true; +} + +bool rtps::Domain::deleteWriter(Participant &part, Writer *writer) { + std::lock_guard lock(m_mutex); + if (writer == nullptr || !writer->isInitialized()) { + return false; + } + if (!part.deleteWriter(writer)) { + return false; + } + + writer->reset(); + return true; +} + +void rtps::Domain::printInfo() { + for (unsigned int i = 0; i < m_participants.size(); i++) { + DOMAIN_LOG("Participant {}", i); + m_participants[i].printInfo(); + } +} + +rtps::GuidPrefix_t Domain::generateGuidPrefix(ParticipantId_t id) const { + GuidPrefix_t prefix; +#if defined(ESP_PLATFORM) + uint8_t mac[6] = {0}; + esp_err_t mac_err = esp_read_mac(mac, ESP_MAC_ETH); + if (mac_err != ESP_OK) { + mac_err = esp_read_mac(mac, ESP_MAC_WIFI_STA); + } + if (mac_err == ESP_OK) { + // Make participant GUID unique per board while keeping a stable layout. + prefix.id[0] = mac[0]; + prefix.id[1] = mac[1]; + prefix.id[2] = mac[2]; + prefix.id[3] = mac[3]; + prefix.id[4] = mac[4]; + prefix.id[5] = mac[5]; + prefix.id[6] = static_cast(id); + prefix.id[7] = Config::VENDOR_ID.vendorId[0]; + prefix.id[8] = Config::VENDOR_ID.vendorId[1]; + prefix.id[9] = Config::DOMAIN_ID; + prefix.id[10] = 0xA5; + prefix.id[11] = 0x5A; + return prefix; + } +#endif + + if (Config::BASE_GUID_PREFIX == GUID_RANDOM) { + // Use OS entropy, not rand(): unseeded rand() yields the identical sequence + // in every process, so two host processes would share a GUID prefix and drop + // each other's packets as their own. + std::random_device rd; + for (unsigned int i = 0; i < prefix.id.size(); i++) { + prefix.id[i] = static_cast(rd()); + } + } else { + for (unsigned int i = 0; i < rtps::Config::BASE_GUID_PREFIX.id.size(); i++) { + prefix.id[i] = Config::BASE_GUID_PREFIX.id[i]; + } + } + // Stamp the participant id (and vendor/domain, mirroring the ESP path) so + // multiple participants in one process always get distinct GUID prefixes - + // identical prefixes make peers discard each other's SPDP as "own message". + prefix.id[6] = static_cast(id); + prefix.id[7] = Config::VENDOR_ID.vendorId[0]; + prefix.id[8] = Config::VENDOR_ID.vendorId[1]; + prefix.id[9] = Config::DOMAIN_ID; + return prefix; +} diff --git a/components/rtps_embedded/src/entities/Participant.cpp b/components/rtps_embedded/src/entities/Participant.cpp new file mode 100644 index 0000000000..2317df8239 --- /dev/null +++ b/components/rtps_embedded/src/entities/Participant.cpp @@ -0,0 +1,518 @@ +/* +The MIT License +Copyright (c) 2019 Lehrstuhl Informatik 11 - RWTH Aachen University +Modifications Copyright (c) 2026 ATDev +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: +The above copyright notice and this permission notice shall be included in +all copies or substantial portions of the Software. +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +THE SOFTWARE + +This file is part of embeddedRTPS. + +Author: i11 - Embedded Software, RWTH Aachen University +*/ + +#include "rtps/entities/Participant.hpp" +#include "rtps/entities/Reader.hpp" +#include "rtps/entities/Writer.hpp" +#include "rtps/messages/MessageReceiver.hpp" +#include "rtps/utils/Log.hpp" +#include + +#if PARTICIPANT_VERBOSE && RTPS_GLOBAL_VERBOSE +#define PARTICIPANT_LOG(...) logger_.warn(__VA_ARGS__) +#else +#define PARTICIPANT_LOG(...) \ + do { \ + } while (0) +#endif + +using rtps::Participant; + +Participant::Participant() + : espp::BaseComponent("RtpsParticipant", espp::Logger::Verbosity::WARN) + , m_guidPrefix(GUIDPREFIX_UNKNOWN) + , m_participantId(PARTICIPANT_ID_INVALID) + , m_receiver(this) {} +Participant::Participant(const GuidPrefix_t &guidPrefix, ParticipantId_t participantId) + : espp::BaseComponent("RtpsParticipant", espp::Logger::Verbosity::WARN) + , m_guidPrefix(guidPrefix) + , m_participantId(participantId) + , m_receiver(this) {} + +Participant::~Participant() { m_spdpAgent.stop(); } + +void Participant::reuse(const GuidPrefix_t &guidPrefix, ParticipantId_t participantId) { + m_guidPrefix = guidPrefix; + m_participantId = participantId; + m_localIpAddress = {Config::IP_ADDRESS[0], Config::IP_ADDRESS[1], Config::IP_ADDRESS[2], + Config::IP_ADDRESS[3]}; +} + +void Participant::reuse(const GuidPrefix_t &guidPrefix, ParticipantId_t participantId, + const Ip4AddressBytes &localIpAddress) { + m_guidPrefix = guidPrefix; + m_participantId = participantId; + m_localIpAddress = localIpAddress; +} + +bool Participant::isValid() { return m_participantId != PARTICIPANT_ID_INVALID; } + +std::array Participant::getNextUserEntityKey() { + const auto result = m_nextUserEntityId; + + ++m_nextUserEntityId[2]; + if (m_nextUserEntityId[2] == 0) { + ++m_nextUserEntityId[1]; + if (m_nextUserEntityId[1] == 0) { + ++m_nextUserEntityId[0]; + } + } + return result; +} + +bool Participant::registerOnNewPublisherMatchedCallback(void (*callback)(void *arg), void *args) { + if (!m_hasBuilInEndpoints) { + return false; + } + + m_sedpAgent.registerOnNewPublisherMatchedCallback(callback, args); + return true; +} + +bool Participant::registerOnNewSubscriberMatchedCallback(void (*callback)(void *arg), void *args) { + if (!m_hasBuilInEndpoints) { + return false; + } + + m_sedpAgent.registerOnNewSubscriberMatchedCallback(callback, args); + return true; +} + +rtps::Writer *Participant::addWriter(Writer *pWriter) { + std::lock_guard lock(m_mutex); + for (unsigned int i = 0; i < m_writers.size(); i++) { + if (m_writers[i] == nullptr) { + m_writers[i] = pWriter; + if (m_hasBuilInEndpoints) { + m_sedpAgent.addWriter(*pWriter); + } + return pWriter; + } + } + return nullptr; +} + +bool Participant::isWritersFull() { + std::lock_guard lock(m_mutex); + for (unsigned int i = 0; i < m_writers.size(); i++) { + if (m_writers[i] == nullptr) { + return false; + } + } + + return true; +} + +rtps::Reader *Participant::addReader(Reader *pReader) { + std::lock_guard lock(m_mutex); + for (unsigned int i = 0; i < m_readers.size(); i++) { + if (m_readers[i] == nullptr) { + m_readers[i] = pReader; + if (m_hasBuilInEndpoints) { + m_sedpAgent.addReader(*pReader); + } + return pReader; + } + } + + return nullptr; +} + +bool Participant::deleteReader(Reader *reader) { + std::lock_guard lock(m_mutex); + for (unsigned int i = 0; i < m_readers.size(); i++) { + if (m_readers[i]->getSEDPSequenceNumber() == reader->getSEDPSequenceNumber()) { + if (m_sedpAgent.deleteReader(reader)) { + m_readers[i] = nullptr; + return true; + } + PARTICIPANT_LOG("Found reader but SEDP deletion failed"); + } + } + return false; +} + +bool Participant::deleteWriter(Writer *writer) { + std::lock_guard lock(m_mutex); + for (unsigned int i = 0; i < m_writers.size(); i++) { + if (m_writers[i]->getSEDPSequenceNumber() == writer->getSEDPSequenceNumber()) { + if (m_sedpAgent.deleteWriter(writer)) { + m_writers[i] = nullptr; + return true; + } + PARTICIPANT_LOG("Found reader but SEDP deletion failed"); + } + } + return false; +} + +bool Participant::isReadersFull() { + std::lock_guard lock(m_mutex); + for (unsigned int i = 0; i < m_readers.size(); i++) { + if (m_readers[i] == nullptr) { + return false; + } + } + + return true; +} + +rtps::Writer *Participant::getWriter(EntityId_t id) { + std::lock_guard lock(m_mutex); + for (size_t i = 0; i < m_writers.size(); ++i) { + if (m_writers[i] == nullptr) { + continue; + } + if (m_writers[i]->m_attributes.endpointGuid.entityId == id) { + return m_writers[i]; + } + } + return nullptr; +} + +rtps::Reader *Participant::getReader(EntityId_t id) { + std::lock_guard lock(m_mutex); + for (size_t i = 0; i < m_readers.size(); ++i) { + if (m_readers[i] == nullptr) { + continue; + } + if (m_readers[i]->m_attributes.endpointGuid.entityId == id) { + return m_readers[i]; + } + } + return nullptr; +} + +rtps::Reader *Participant::getReaderByWriterId(const Guid_t &guid) { + std::lock_guard lock(m_mutex); + for (size_t i = 0; i < m_readers.size(); ++i) { + if (m_readers[i] == nullptr) { + continue; + } + if (m_readers[i]->isProxy(guid)) { + return m_readers[i]; + } + } + return nullptr; +} + +rtps::Writer *Participant::getMatchingWriter(const TopicData &readerTopicData) { + std::lock_guard lock(m_mutex); + for (size_t i = 0; i < m_writers.size(); ++i) { + if (m_writers[i] == nullptr) { + continue; + } + if (m_writers[i]->m_attributes.matchesTopicOf(readerTopicData) && + (readerTopicData.reliabilityKind == ReliabilityKind_t::BEST_EFFORT || + m_writers[i]->m_attributes.reliabilityKind == ReliabilityKind_t::RELIABLE)) { + return m_writers[i]; + } + } + return nullptr; +} + +rtps::Reader *Participant::getMatchingReader(const TopicData &writerTopicData) { + std::lock_guard lock(m_mutex); + for (size_t i = 0; i < m_readers.size(); ++i) { + if (m_readers[i] == nullptr) { + continue; + } + if (m_readers[i]->m_attributes.matchesTopicOf(writerTopicData) && + (writerTopicData.reliabilityKind == ReliabilityKind_t::RELIABLE || + m_readers[i]->m_attributes.reliabilityKind == ReliabilityKind_t::BEST_EFFORT)) { + return m_readers[i]; + } + } + return nullptr; +} + +rtps::Writer *Participant::getMatchingWriter(const TopicDataCompressed &readerTopicData) { + std::lock_guard lock(m_mutex); + for (size_t i = 0; i < m_writers.size(); ++i) { + if (m_writers[i] == nullptr) { + continue; + } + if (readerTopicData.matchesTopicOf(m_writers[i]->m_attributes) && + (readerTopicData.is_reliable == false || + m_writers[i]->m_attributes.reliabilityKind == ReliabilityKind_t::RELIABLE)) { + return m_writers[i]; + } + } + return nullptr; +} + +rtps::Reader *Participant::getMatchingReader(const TopicDataCompressed &writerTopicData) { + std::lock_guard lock(m_mutex); + for (size_t i = 0; i < m_readers.size(); ++i) { + if (m_readers[i] == nullptr) { + continue; + } + if (writerTopicData.matchesTopicOf(m_readers[i]->m_attributes) && + (writerTopicData.is_reliable == true || + m_readers[i]->m_attributes.reliabilityKind == ReliabilityKind_t::BEST_EFFORT)) { + return m_readers[i]; + } + } + return nullptr; +} + +bool Participant::addNewRemoteParticipant(const ParticipantProxyData &remotePart) { + std::lock_guard lock(m_mutex); + return m_remoteParticipants.add(remotePart); +} + +bool Participant::removeRemoteParticipant(const GuidPrefix_t &prefix) { + std::lock_guard lock(m_mutex); + auto isElementToRemove = [&](const ParticipantProxyData &proxy) { + return proxy.m_guid.prefix == prefix; + }; + auto thunk = [](void *arg, const ParticipantProxyData &value) { + return (*static_cast(arg))(value); + }; + removeAllProxiesOfParticipant(prefix); + m_sedpAgent.removeUnmatchedEntitiesOfParticipant(prefix); + return m_remoteParticipants.remove(thunk, &isElementToRemove); +} + +void Participant::removeAllProxiesOfParticipant(const GuidPrefix_t &prefix) { + std::lock_guard lock(m_mutex); + for (unsigned int i = 0; i < m_readers.size(); i++) { + if (m_readers[i] == nullptr) { + continue; + } + m_readers[i]->removeAllProxiesOfParticipant(prefix); + } + + for (unsigned int i = 0; i < m_writers.size(); i++) { + if (m_writers[i] == nullptr) { + continue; + } + m_writers[i]->removeAllProxiesOfParticipant(prefix); + } +} + +void Participant::removeProxyFromAllEndpoints(const Guid_t &guid) { + std::lock_guard lock(m_mutex); + for (unsigned int i = 0; i < m_writers.size(); i++) { + if (m_writers[i] == nullptr) { + continue; + } + if (m_writers[i]->removeProxy(guid)) { + PARTICIPANT_LOG("Removing proxy for writer [{}, {}], proxies left = {}", + m_writers[i]->m_attributes.topicName, m_writers[i]->m_attributes.typeName, + (int)m_writers[i]->getProxiesCount()); + } + } + + for (unsigned int i = 0; i < m_readers.size(); i++) { + if (m_readers[i] == nullptr) { + continue; + } + if (m_readers[i]->removeProxy(guid)) { + PARTICIPANT_LOG("Removing proxy for reader [{}, {}], proxies left = {}", + m_readers[i]->m_attributes.topicName, m_readers[i]->m_attributes.typeName, + (int)m_readers[i]->getProxiesCount()); + } + } +} + +const rtps::ParticipantProxyData *Participant::findRemoteParticipant(const GuidPrefix_t &prefix) { + std::lock_guard lock(m_mutex); + auto isElementToFind = [&](const ParticipantProxyData &proxy) { + return proxy.m_guid.prefix == prefix; + }; + auto thunk = [](void *arg, const ParticipantProxyData &value) { + return (*static_cast(arg))(value); + }; + return m_remoteParticipants.find(thunk, &isElementToFind); +} + +void Participant::refreshRemoteParticipantLiveliness(const GuidPrefix_t &prefix) { + std::lock_guard lock(m_mutex); + auto isElementToFind = [&](const ParticipantProxyData &proxy) { + return proxy.m_guid.prefix == prefix; + }; + auto thunk = [](void *arg, const ParticipantProxyData &value) { + return (*static_cast(arg))(value); + }; + + auto remoteParticipant = m_remoteParticipants.find(thunk, &isElementToFind); + if (remoteParticipant != nullptr) { + remoteParticipant->onAliveSignal(); + } +} + +bool Participant::hasReaderWithMulticastLocator(const std::array &address) { + std::lock_guard lock(m_mutex); + for (size_t i = 0; i < m_readers.size(); i++) { + if (m_readers[i] == nullptr) { + continue; + } + if (m_readers[i]->m_attributes.multicastLocator.getIp4AddressBytes() == address) { + return true; + } + } + return false; +} + +uint32_t Participant::getRemoteParticipantCount() { + std::lock_guard lock(m_mutex); + return m_remoteParticipants.getNumElements(); +} + +rtps::MessageReceiver *Participant::getMessageReceiver() { return &m_receiver; } + +bool Participant::checkAndResetHeartbeats() { + std::lock_guard lock1(m_mutex); + std::lock_guard lock2(m_spdpAgent.m_mutex); + PARTICIPANT_LOG("Have {} remote participants", + (unsigned int)m_remoteParticipants.getNumElements()); + PARTICIPANT_LOG("Unmatched remote writers/readers, {} / {}", + static_cast(m_sedpAgent.getNumRemoteUnmatchedWriters()), + static_cast(m_sedpAgent.getNumRemoteUnmatchedReaders())); + for (auto &remote : m_remoteParticipants) { + PARTICIPANT_LOG("Remote GUID = {} {} {} {} | Age = {} [ms]", remote.m_guid.prefix.id[4], + remote.m_guid.prefix.id[5], remote.m_guid.prefix.id[6], + remote.m_guid.prefix.id[7], + (unsigned int)remote.getAliveSignalAgeInMilliseconds()); + if (remote.isAlive()) { + continue; + } + PARTICIPANT_LOG("removing remote participant"); + bool success = removeRemoteParticipant(remote.m_guid.prefix); + if (!success) { + return false; + } else { + return true; + } + } + return true; +} + +void Participant::printInfo() { + + uint32_t max_reader_proxies = 0; + for (unsigned int i = 0; i < m_readers.size(); i++) { + if (m_readers[i] != nullptr && m_readers[i]->isInitialized()) { + if (m_hasBuilInEndpoints && i < 3) { +#ifdef PARTICIPANT_PRINTINFO_LONG + if (m_readers[i]->m_attributes.endpointGuid.entityId == + ENTITYID_SPDP_BUILTIN_PARTICIPANT_READER) { + PARTICIPANT_LOG("Reader {}: SPDP BUILTIN READER | Remote Proxies = {}", i, + static_cast(m_readers[i]->getProxiesCount())); + } + if (m_readers[i]->m_attributes.endpointGuid.entityId == + ENTITYID_SEDP_BUILTIN_PUBLICATIONS_READER) { + PARTICIPANT_LOG("Reader {}: SEDP PUBLICATION READER | Remote Proxies = {}", i, + static_cast(m_readers[i]->getProxiesCount())); + } + if (m_readers[i]->m_attributes.endpointGuid.entityId == + ENTITYID_SEDP_BUILTIN_SUBSCRIPTIONS_READER) { + PARTICIPANT_LOG("Reader {}: SEDP SUBSCRIPTION READER | Remote Proxies = {}", i, + static_cast(m_readers[i]->getProxiesCount())); + } +#endif + continue; + } + + max_reader_proxies = std::max(max_reader_proxies, m_readers[i]->getProxiesCount()); +#ifdef PARTICIPANT_PRINTINFO_LONG + PARTICIPANT_LOG("Reader {}: Topic = {} | Type = {} | Remote Proxies = {} | SEDP " + "SN = {}", + i, m_readers[i]->m_attributes.topicName, m_readers[i]->m_attributes.typeName, + static_cast(m_readers[i]->getProxiesCount()), + static_cast(m_readers[i]->getSEDPSequenceNumber().low)); +#endif + } + } + + uint32_t max_writer_proxies = 0; + for (unsigned int i = 0; i < m_writers.size(); i++) { + + if (m_hasBuilInEndpoints && i < 3) { +#ifdef PARTICIPANT_PRINTINFO_LONG + if (m_writers[i]->m_attributes.endpointGuid.entityId == + ENTITYID_SPDP_BUILTIN_PARTICIPANT_WRITER) { + PARTICIPANT_LOG("Writer {}: SPDP WRITER | Remote Proxies = {}", i, + static_cast(m_writers[i]->getProxiesCount())); + } + if (m_writers[i]->m_attributes.endpointGuid.entityId == + ENTITYID_SEDP_BUILTIN_PUBLICATIONS_WRITER) { + PARTICIPANT_LOG("Writer {}: SEDP PUBLICATION WRITER | Remote Proxies = {}", i, + static_cast(m_writers[i]->getProxiesCount())); + } + if (m_writers[i]->m_attributes.endpointGuid.entityId == + ENTITYID_SEDP_BUILTIN_SUBSCRIPTIONS_WRITER) { + PARTICIPANT_LOG("Writer {}: SEDP SUBSCRIPTION WRITER | Remote Proxies = {}", i, + static_cast(m_writers[i]->getProxiesCount())); + } +#endif + continue; + } + + if (m_writers[i] != nullptr && m_writers[i]->isInitialized()) { + max_writer_proxies = std::max(max_writer_proxies, m_writers[i]->getProxiesCount()); +#ifdef PARTICIPANT_PRINTINFO_LONG + PARTICIPANT_LOG("Writer {}: Topic = {} | Type = {} | Remote Proxies = {} | SEDP " + "SN = {}", + i, m_writers[i]->m_attributes.topicName, m_writers[i]->m_attributes.typeName, + static_cast(m_writers[i]->getProxiesCount()), + static_cast(m_writers[i]->getSEDPSequenceNumber().low)); +#endif + } + } + + PARTICIPANT_LOG("Max Writer Proxies {}", max_writer_proxies); + PARTICIPANT_LOG("Max Reader Proxies {}", max_reader_proxies); + PARTICIPANT_LOG("Unmatched Remote Readers = {}", + static_cast(m_sedpAgent.getNumRemoteUnmatchedReaders())); + PARTICIPANT_LOG("Unmatched Remote Writers = {}", + static_cast(m_sedpAgent.getNumRemoteUnmatchedWriters())); + PARTICIPANT_LOG("Remote Participants = {}", + static_cast(m_remoteParticipants.getNumElements())); +} + +rtps::SPDPAgent &Participant::getSPDPAgent() { return m_spdpAgent; } + +void Participant::addBuiltInEndpoints(BuiltInEndpoints &endpoints) { + std::lock_guard lock(m_mutex); + m_hasBuilInEndpoints = true; + m_spdpAgent.init(*this, endpoints); + m_sedpAgent.init(*this, endpoints); + + // This needs to be done after initializing the agents + addWriter(endpoints.spdpWriter); + addReader(endpoints.spdpReader); + addWriter(endpoints.sedpPubWriter); + addReader(endpoints.sedpPubReader); + addWriter(endpoints.sedpSubWriter); + addReader(endpoints.sedpSubReader); +} + +void Participant::newMessage(const uint8_t *data, DataSize_t size) { + if (!m_receiver.processMessage(data, size)) { + PARTICIPANT_LOG("MESSAGE PROCESSING FAILED"); + } +} diff --git a/components/rtps_embedded/src/entities/Reader.cpp b/components/rtps_embedded/src/entities/Reader.cpp new file mode 100644 index 0000000000..f2406955c0 --- /dev/null +++ b/components/rtps_embedded/src/entities/Reader.cpp @@ -0,0 +1,144 @@ +#include +#include +#include +#include +#include +#include + +using namespace rtps; + +Reader::Reader() + : espp::BaseComponent("RtpsReader", espp::Logger::Verbosity::WARN) { + m_callbacks.fill({nullptr, nullptr, 0}); +} + +void Reader::executeCallbacks(const ReaderCacheChange &cacheChange) { + std::lock_guard lock(m_callback_mutex); + for (unsigned int i = 0; i < m_callbacks.size(); i++) { + if (m_callbacks[i].function != nullptr) { + m_callbacks[i].function(m_callbacks[i].arg, cacheChange); + } + } +} + +bool Reader::initMutex() { return true; } + +void Reader::reset() { + std::lock_guard lock1(m_proxies_mutex); + std::lock_guard lock2(m_callback_mutex); + + m_proxies.clear(); + for (unsigned int i = 0; i < m_callbacks.size(); i++) { + m_callbacks[i].function = nullptr; + m_callbacks[i].arg = nullptr; + } + + m_callback_count = 0; + m_is_initialized_ = false; +} + +bool Reader::isProxy(const Guid_t &guid) { + for (const auto &proxy : m_proxies) { + if (proxy.remoteWriterGuid.operator==(guid)) { + return true; + } + } + return false; +} + +WriterProxy *Reader::getProxy(Guid_t guid) { + auto isElementToFind = [&](const WriterProxy &proxy) { return proxy.remoteWriterGuid == guid; }; + auto thunk = [](void *arg, const WriterProxy &value) { + return (*static_cast(arg))(value); + }; + return m_proxies.find(thunk, &isElementToFind); +} + +Reader::callbackIdentifier_t Reader::registerCallback(Reader::callbackFunction_t cb, void *arg) { + std::lock_guard lock(m_callback_mutex); + if (m_callback_count == m_callbacks.size() || cb == nullptr) { + return false; + } + + for (unsigned int i = 0; i < m_callbacks.size(); i++) { + if (m_callbacks[i].function == nullptr) { + m_callbacks[i].function = cb; + m_callbacks[i].arg = arg; + m_callbacks[i].identifier = m_callback_identifier++; + m_callback_count++; + return m_callbacks[i].identifier; + } + } + + return 0; +} + +uint32_t Reader::getProxiesCount() { return m_proxies.getNumElements(); } + +bool Reader::removeCallback(Reader::callbackIdentifier_t identifier) { + std::lock_guard lock(m_callback_mutex); + for (unsigned int i = 0; i < m_callbacks.size(); i++) { + if (m_callbacks[i].identifier == identifier) { + m_callbacks[i].function = nullptr; + m_callbacks[i].arg = nullptr; + m_callback_count--; + return true; + } + } + + return false; +} + +uint8_t Reader::getNumCallbacks() { return m_callback_count; } + +void Reader::removeAllProxiesOfParticipant(const GuidPrefix_t &guidPrefix) { + std::lock_guard lock(m_proxies_mutex); + auto isElementToRemove = [&](const WriterProxy &proxy) { + return proxy.remoteWriterGuid.prefix == guidPrefix; + }; + auto thunk = [](void *arg, const WriterProxy &value) { + return (*static_cast(arg))(value); + }; + + m_proxies.remove(thunk, &isElementToRemove); +} + +bool Reader::removeProxy(const Guid_t &guid) { + std::lock_guard lock(m_proxies_mutex); + auto isElementToRemove = [&](const WriterProxy &proxy) { return proxy.remoteWriterGuid == guid; }; + auto thunk = [](void *arg, const WriterProxy &value) { + return (*static_cast(arg))(value); + }; + + return m_proxies.remove(thunk, &isElementToRemove); +} + +bool Reader::addNewMatchedWriter(const WriterProxy &newProxy) { + std::lock_guard lock(m_proxies_mutex); +#if (SFR_VERBOSE || SLR_VERBOSE) && RTPS_GLOBAL_VERBOSE + SFR_LOG("New writer added with id: "); + printGuid(newProxy.remoteWriterGuid); +#endif + return m_proxies.add(newProxy); +} + +void rtps::Reader::setSEDPSequenceNumber(const SequenceNumber_t &sn) { + m_sedp_sequence_number = sn; +} +const rtps::SequenceNumber_t &rtps::Reader::getSEDPSequenceNumber() { + return m_sedp_sequence_number; +} + +int rtps::Reader::dumpAllProxies(dumpProxyCallback target, void *arg) { + if (target == nullptr) { + return 0; + } + std::lock_guard lock(m_proxies_mutex); + int dump_count = 0; + for (auto it = m_proxies.begin(); it != m_proxies.end(); ++it, ++dump_count) { + target(this, *it, arg); + } + return dump_count; +} + +bool rtps::Reader::sendPreemptiveAckNack(const WriterProxy &writer) { return true; } diff --git a/components/rtps_embedded/src/entities/StatelessReader.cpp b/components/rtps_embedded/src/entities/StatelessReader.cpp new file mode 100644 index 0000000000..c63799a9da --- /dev/null +++ b/components/rtps_embedded/src/entities/StatelessReader.cpp @@ -0,0 +1,78 @@ +/* +The MIT License +Copyright (c) 2019 Lehrstuhl Informatik 11 - RWTH Aachen University +Modifications Copyright (c) 2026 ATDev +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: +The above copyright notice and this permission notice shall be included in +all copies or substantial portions of the Software. +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +THE SOFTWARE + +This file is part of embeddedRTPS. + +Author: i11 - Embedded Software, RWTH Aachen University +*/ + +#include "rtps/entities/StatelessReader.hpp" +#include "rtps/utils/Log.hpp" + +using rtps::StatelessReader; + +#if SLR_VERBOSE && RTPS_GLOBAL_VERBOSE +#include "rtps/utils/printutils.hpp" +#define SLR_LOG(...) \ + if (true) { \ + printf("[StatelessReader %s] ", &m_attributes.topicName[0]); \ + printf(__VA_ARGS__); \ + printf("\n"); \ + } +#else +#define SLR_LOG(...) // +#endif + +bool StatelessReader::init(const TopicData &attributes) { + if (!initMutex()) { + return false; + } + + m_proxies.clear(); + m_attributes = attributes; + m_is_initialized_ = true; + return true; +} + +void StatelessReader::newChange(const ReaderCacheChange &cacheChange) { + if (!m_is_initialized_) { + return; + } + executeCallbacks(cacheChange); +} + +bool StatelessReader::addNewMatchedWriter(const WriterProxy &newProxy) { +#if (SLR_VERBOSE && RTPS_GLOBAL_VERBOSE) + SLR_LOG("Adding WriterProxy"); + printGuid(newProxy.remoteWriterGuid); +#endif + return m_proxies.add(newProxy); +} + +bool StatelessReader::onNewHeartbeat(const SubmessageHeartbeat &, const GuidPrefix_t &) { + // nothing to do + return true; +} + +bool StatelessReader::onNewGapMessage(const SubmessageGap &msg, const GuidPrefix_t &remotePrefix) { + return true; +} + +#undef SLR_VERBOSE diff --git a/components/rtps_embedded/src/entities/Writer.cpp b/components/rtps_embedded/src/entities/Writer.cpp new file mode 100644 index 0000000000..6d1e2ff379 --- /dev/null +++ b/components/rtps_embedded/src/entities/Writer.cpp @@ -0,0 +1,144 @@ +#include "rtps/utils/Log.hpp" +#include +#include +#include +#include +#include +#include + +using namespace rtps; + +Writer::Writer() + : espp::BaseComponent("RtpsWriter", espp::Logger::Verbosity::WARN) {} + +bool rtps::Writer::addNewMatchedReader(const ReaderProxy &newProxy) { + INIT_GUARD(); +#if SFW_VERBOSE && RTPS_GLOBAL_VERBOSE + SFW_LOG("New reader added with id: "); + printGuid(newProxy.remoteReaderGuid); +#endif + std::lock_guard lock(m_mutex); + bool success = m_proxies.add(newProxy); + if (!m_enforceUnicast) { + manageSendOptions(); + } + return success; +} + +bool rtps::Writer::removeProxy(const Guid_t &guid) { + INIT_GUARD() + std::lock_guard lock(m_mutex); + auto isElementToRemove = [&](const ReaderProxy &proxy) { return proxy.remoteReaderGuid == guid; }; + auto thunk = [](void *arg, const ReaderProxy &value) { + return (*static_cast(arg))(value); + }; + + bool ret = m_proxies.remove(thunk, &isElementToRemove); + resetSendOptions(); + return ret; +} + +uint32_t rtps::Writer::getProxiesCount() { + std::lock_guard lock(m_mutex); + return m_proxies.getNumElements(); +} + +void rtps::Writer::resetSendOptions() { + INIT_GUARD() + for (auto &proxy : m_proxies) { + proxy.suppressUnicast = false; + proxy.useMulticast = false; + proxy.unknown_eid = false; + } + manageSendOptions(); +} + +const rtps::CacheChange *rtps::Writer::newChange(ChangeKind_t kind, const uint8_t *data, + DataSize_t size) { + return newChange(kind, data, size, false, false); +} + +void rtps::Writer::manageSendOptions() { + INIT_GUARD(); + std::lock_guard lock(m_mutex); + for (auto &proxy : m_proxies) { + if (proxy.remoteMulticastLocator.kind == LocatorKind_t::LOCATOR_KIND_INVALID) { + proxy.suppressUnicast = false; + proxy.useMulticast = false; + } else { + bool found = false; + for (auto &avproxy : m_proxies) { + if (avproxy.remoteMulticastLocator.kind == LocatorKind_t::LOCATOR_KIND_UDPv4 && + avproxy.remoteMulticastLocator.getIp4AddressBytes() == + proxy.remoteMulticastLocator.getIp4AddressBytes() && + avproxy.remoteLocator.getIp4AddressBytes() != + proxy.remoteLocator.getIp4AddressBytes()) { + if (avproxy.suppressUnicast == false) { + avproxy.useMulticast = false; + avproxy.suppressUnicast = true; + proxy.useMulticast = true; + proxy.suppressUnicast = true; + if (avproxy.remoteReaderGuid.entityId != proxy.remoteReaderGuid.entityId) { + proxy.unknown_eid = true; + } + } + found = true; + } + } + if (!found) { + proxy.useMulticast = false; + proxy.suppressUnicast = false; + } + } + } +} + +void rtps::Writer::removeAllProxiesOfParticipant(const GuidPrefix_t &guidPrefix) { + INIT_GUARD(); + std::lock_guard lock(m_mutex); + auto isElementToRemove = [&](const ReaderProxy &proxy) { + return proxy.remoteReaderGuid.prefix == guidPrefix; + }; + auto thunk = [](void *arg, const ReaderProxy &value) { + return (*static_cast(arg))(value); + }; + + m_proxies.remove(thunk, &isElementToRemove); + resetSendOptions(); +} + +bool rtps::Writer::isBuiltinEndpoint() { + return !(m_attributes.endpointGuid.entityId.entityKind == + EntityKind_t::USER_DEFINED_WRITER_WITHOUT_KEY || + m_attributes.endpointGuid.entityId.entityKind == + EntityKind_t::USER_DEFINED_WRITER_WITH_KEY); +} + +bool rtps::Writer::isIrrelevant(ChangeKind_t kind) const { + // Right now we only allow alive changes + // return kind == ChangeKind_t::INVALID || (m_topicKind == TopicKind_t::NO_KEY + // && kind != ChangeKind_t::ALIVE); + return kind != ChangeKind_t::ALIVE; +} + +bool rtps::Writer::isInitialized() { return m_is_initialized_; } + +void rtps::Writer::setSEDPSequenceNumber(const SequenceNumber_t &sn) { + m_sedp_sequence_number = sn; +} + +const rtps::SequenceNumber_t &rtps::Writer::getSEDPSequenceNumber() { + return m_sedp_sequence_number; +} + +int rtps::Writer::dumpAllProxies(dumpProxyCallback target, void *arg) { + if (target == nullptr) { + return 0; + } + std::lock_guard lock(m_mutex); + int dump_count = 0; + for (auto it = m_proxies.begin(); it != m_proxies.end(); ++it, ++dump_count) { + target(this, *it, arg); + } + return dump_count; +} diff --git a/components/rtps_embedded/src/messages/MessageReceiver.cpp b/components/rtps_embedded/src/messages/MessageReceiver.cpp new file mode 100644 index 0000000000..46d11ad2d1 --- /dev/null +++ b/components/rtps_embedded/src/messages/MessageReceiver.cpp @@ -0,0 +1,279 @@ +/* +The MIT License +Copyright (c) 2019 Lehrstuhl Informatik 11 - RWTH Aachen University +Modifications Copyright (c) 2026 ATDev +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: +The above copyright notice and this permission notice shall be included in +all copies or substantial portions of the Software. +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +THE SOFTWARE + +This file is part of embeddedRTPS. + +Author: i11 - Embedded Software, RWTH Aachen University +*/ + +#include "rtps/messages/MessageReceiver.hpp" +#include + +#include "rtps/entities/Reader.hpp" +#include "rtps/entities/Writer.hpp" +#include "rtps/messages/MessageTypes.hpp" +#include "rtps/utils/Log.hpp" + +#include + +using rtps::MessageReceiver; + +#if RECV_VERBOSE && RTPS_GLOBAL_VERBOSE +#include "rtps/utils/printutils.hpp" +#define RECV_LOG(...) logger_.warn(__VA_ARGS__) +#else +#define RECV_LOG(...) \ + do { \ + } while (0) +#endif + +MessageReceiver::MessageReceiver(Participant *part) + : espp::BaseComponent("RtpsMessageReceiver", espp::Logger::Verbosity::WARN) + , mp_part(part) {} + +bool MessageReceiver::processMessage(const uint8_t *data, DataSize_t size) { + rtps::MessageSourceState sourceState; + MessageProcessingInfo msgInfo(data, size); + + if (!processHeader(msgInfo, sourceState)) { + return false; + } + SubmessageHeader submsgHeader; + while (msgInfo.nextPos < msgInfo.size) { + if (!deserializeMessage(msgInfo, submsgHeader)) { + return false; + } + processSubmessage(msgInfo, submsgHeader, sourceState); + } + + return true; +} + +bool MessageReceiver::processHeader(MessageProcessingInfo &msgInfo, + rtps::MessageSourceState &sourceState) { + Header header; + if (!deserializeMessage(msgInfo, header)) { + return false; + } + + if (header.guidPrefix.id == mp_part->m_guidPrefix.id) { + RECV_LOG("[MessageReceiver]: Received own message."); + return false; // Don't process our own packet + } + + if (header.protocolName != RTPS_PROTOCOL_NAME || + header.protocolVersion.major != PROTOCOLVERSION.major) { + return false; + } + + sourceState.sourceGuidPrefix = header.guidPrefix; + sourceState.sourceVendor = header.vendorId; + sourceState.sourceVersion = header.protocolVersion; + + msgInfo.nextPos += Header::getRawSize(); + return true; +} + +bool MessageReceiver::processSubmessage(MessageProcessingInfo &msgInfo, + const SubmessageHeader &submsgHeader, + const rtps::MessageSourceState &sourceState) { + bool success = false; + + switch (submsgHeader.submessageId) { + case SubmessageKind::ACKNACK: + RECV_LOG("Processing AckNack submessage"); + success = processAckNackSubmessage(msgInfo, sourceState); + break; + case SubmessageKind::DATA: + RECV_LOG("Processing Data submessage"); + success = processDataSubmessage(msgInfo, submsgHeader, sourceState); + break; + case SubmessageKind::HEARTBEAT: + RECV_LOG("Processing Heartbeat submessage"); + success = processHeartbeatSubmessage(msgInfo, sourceState); + break; + case SubmessageKind::INFO_DST: + RECV_LOG("Info_DST submessage not relevant."); + success = true; // Not relevant + break; + case SubmessageKind::GAP: + RECV_LOG("Processing GAP submessage"); + success = processGapSubmessage(msgInfo, sourceState); + break; + case SubmessageKind::INFO_TS: + RECV_LOG("Info_TS submessage not relevant."); + success = true; // Not relevant now + break; + default: + RECV_LOG("Submessage of type {} currently not supported. Skipping..", + static_cast(submsgHeader.submessageId)); + success = false; + } + msgInfo.nextPos += submsgHeader.octetsToNextHeader + SubmessageHeader::getRawSize(); + return success; +} + +bool MessageReceiver::processDataSubmessage(MessageProcessingInfo &msgInfo, + const SubmessageHeader &submsgHeader, + const rtps::MessageSourceState &sourceState) { + SubmessageData dataSubmsg; + if (!deserializeMessage(msgInfo, dataSubmsg)) { + return false; + } + + const uint8_t *submessageStart = msgInfo.getPointerToCurrentPos(); + const uint8_t *submessageEnd = + submessageStart + SubmessageHeader::getRawSize() + submsgHeader.octetsToNextHeader; + const uint16_t submessageBodyOffset = + static_cast(sizeof(dataSubmsg.extraFlags) + sizeof(dataSubmsg.octetsToInlineQos) + + dataSubmsg.octetsToInlineQos); + const uint8_t *serializedData = + submessageStart + SubmessageHeader::getRawSize() + submessageBodyOffset; + + if (serializedData > submessageEnd) { + return false; + } + + if ((submsgHeader.flags & FLAG_INLINE_QOS) != 0) { + const uint8_t *cursor = serializedData; + bool foundSentinel = false; + + while ((cursor + sizeof(uint16_t) + sizeof(uint16_t)) <= submessageEnd) { + uint16_t pid = 0; + uint16_t length = 0; + memcpy(&pid, cursor, sizeof(pid)); + cursor += sizeof(pid); + memcpy(&length, cursor, sizeof(length)); + cursor += sizeof(length); + + if (pid == SMElement::PID_SENTINEL) { + foundSentinel = true; + serializedData = cursor; + break; + } + + if (cursor + length > submessageEnd) { + return false; + } + cursor += length; + + const std::size_t consumed = static_cast(cursor - serializedData); + const std::size_t alignment = (4 - (consumed % 4)) % 4; + if (cursor + alignment > submessageEnd) { + return false; + } + cursor += alignment; + } + + if (!foundSentinel) { + return false; + } + } + + const DataSize_t size = static_cast(submessageEnd - serializedData); + + RECV_LOG("Received data message size {}", static_cast(size)); + + Reader *reader; + if (dataSubmsg.readerId == ENTITYID_UNKNOWN) { +#if RECV_VERBOSE && RTPS_GLOBAL_VERBOSE + RECV_LOG("Received ENTITYID_UNKNOWN readerID, searching for writer ID = "); + printGuid(Guid_t{sourceState.sourceGuidPrefix, dataSubmsg.writerId}); +#endif + reader = + mp_part->getReaderByWriterId(Guid_t{sourceState.sourceGuidPrefix, dataSubmsg.writerId}); + if (reader != nullptr) + RECV_LOG("Found reader!"); + } else { + reader = mp_part->getReader(dataSubmsg.readerId); +#if RECV_VERBOSE && RTPS_GLOBAL_VERBOSE + auto reader_by_writer = + mp_part->getReaderByWriterId(Guid_t{sourceState.sourceGuidPrefix, dataSubmsg.writerId}); + + if (reader_by_writer == nullptr && reader != nullptr) { + RECV_LOG("FOUND By READER ID, NOT BY WRITER ID ="); + printGuid(Guid_t{sourceState.sourceGuidPrefix, dataSubmsg.writerId}); + } +#endif + } + if (reader != nullptr) { + Guid_t writerGuid{sourceState.sourceGuidPrefix, dataSubmsg.writerId}; + ReaderCacheChange change{ChangeKind_t::ALIVE, writerGuid, dataSubmsg.writerSN, serializedData, + size}; + reader->newChange(change); + } else { +#if RECV_VERBOSE && RTPS_GLOBAL_VERBOSE + RECV_LOG("Couldn't find a reader with id: "); + printEntityId(dataSubmsg.readerId); +#endif + } + + return true; +} + +bool MessageReceiver::processHeartbeatSubmessage(MessageProcessingInfo &msgInfo, + const rtps::MessageSourceState &sourceState) { + SubmessageHeartbeat submsgHB; + if (!deserializeMessage(msgInfo, submsgHB)) { + return false; + } + + Reader *reader = mp_part->getReader(submsgHB.readerId); + if (reader != nullptr) { + reader->onNewHeartbeat(submsgHB, sourceState.sourceGuidPrefix); + mp_part->refreshRemoteParticipantLiveliness(sourceState.sourceGuidPrefix); + return true; + } else { + return false; + } +} + +bool MessageReceiver::processAckNackSubmessage(MessageProcessingInfo &msgInfo, + const rtps::MessageSourceState &sourceState) { + SubmessageAckNack submsgAckNack; + if (!deserializeMessage(msgInfo, submsgAckNack)) { + return false; + } + + Writer *writer = mp_part->getWriter(submsgAckNack.writerId); + if (writer != nullptr) { + writer->onNewAckNack(submsgAckNack, sourceState.sourceGuidPrefix); + return true; + } else { + return false; + } +} + +bool MessageReceiver::processGapSubmessage(MessageProcessingInfo &msgInfo, + const rtps::MessageSourceState &sourceState) { + SubmessageGap submsgGap; + if (!deserializeMessage(msgInfo, submsgGap)) { + return false; + } + + Reader *reader = mp_part->getReader(submsgGap.readerId); + if (reader != nullptr) { + reader->onNewGapMessage(submsgGap, sourceState.sourceGuidPrefix); + return true; + } else { + return false; + } +} +#undef RECV_VERBOSE diff --git a/components/rtps_embedded/src/messages/MessageTypes.cpp b/components/rtps_embedded/src/messages/MessageTypes.cpp new file mode 100644 index 0000000000..8ed25a2dfe --- /dev/null +++ b/components/rtps_embedded/src/messages/MessageTypes.cpp @@ -0,0 +1,199 @@ +/* +The MIT License +Copyright (c) 2019 Lehrstuhl Informatik 11 - RWTH Aachen University +Modifications Copyright (c) 2026 ATDev +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: +The above copyright notice and this permission notice shall be included in +all copies or substantial portions of the Software. +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +THE SOFTWARE + +This file is part of embeddedRTPS. + +Author: i11 - Embedded Software, RWTH Aachen University +*/ + +#include "rtps/messages/MessageTypes.hpp" + +#include +#include + +#include +using namespace rtps; + +void doCopyAndMoveOn(uint8_t *dst, const uint8_t *&src, size_t size) { + memcpy(dst, src, size); + src += size; +} + +bool rtps::deserializeMessage(const MessageProcessingInfo &info, Header &header) { + if (info.getRemainingSize() < Header::getRawSize()) { + return false; + } + + const uint8_t *currentPos = info.getPointerToCurrentPos(); + doCopyAndMoveOn(header.protocolName.data(), currentPos, sizeof(std::array)); + doCopyAndMoveOn(reinterpret_cast(&header.protocolVersion), currentPos, + sizeof(ProtocolVersion_t)); + doCopyAndMoveOn(header.vendorId.vendorId.data(), currentPos, header.vendorId.vendorId.size()); + doCopyAndMoveOn(header.guidPrefix.id.data(), currentPos, header.guidPrefix.id.size()); + return true; +} + +bool rtps::deserializeMessage(const MessageProcessingInfo &info, SubmessageHeader &header) { + if (info.getRemainingSize() < SubmessageHeader::getRawSize()) { + return false; + } + + const uint8_t *currentPos = info.getPointerToCurrentPos(); + header.submessageId = static_cast(*currentPos++); + header.flags = *(currentPos++); + doCopyAndMoveOn(reinterpret_cast(&header.octetsToNextHeader), currentPos, + sizeof(uint16_t)); + return true; +} + +bool rtps::deserializeMessage(const MessageProcessingInfo &info, SubmessageData &msg) { + if (info.getRemainingSize() < SubmessageHeader::getRawSize()) { + return false; + } + if (!deserializeMessage(info, msg.header)) { + return false; + } + + // Check for length including data + constexpr auto min_body_size = SubmessageData::getRawSize() - SubmessageHeader::getRawSize(); + if (msg.header.octetsToNextHeader < min_body_size) { + return false; + } + if (info.getRemainingSize() < SubmessageHeader::getRawSize() + msg.header.octetsToNextHeader) { + return false; + } + + const uint8_t *currentPos = info.getPointerToCurrentPos() + SubmessageHeader::getRawSize(); + + doCopyAndMoveOn(reinterpret_cast(&msg.extraFlags), currentPos, sizeof(uint16_t)); + doCopyAndMoveOn(reinterpret_cast(&msg.octetsToInlineQos), currentPos, + sizeof(uint16_t)); + doCopyAndMoveOn(msg.readerId.entityKey.data(), currentPos, msg.readerId.entityKey.size()); + msg.readerId.entityKind = static_cast(*currentPos++); + doCopyAndMoveOn(msg.writerId.entityKey.data(), currentPos, msg.writerId.entityKey.size()); + msg.writerId.entityKind = static_cast(*currentPos++); + doCopyAndMoveOn(reinterpret_cast(&msg.writerSN.high), currentPos, + sizeof(msg.writerSN.high)); + doCopyAndMoveOn(reinterpret_cast(&msg.writerSN.low), currentPos, + sizeof(msg.writerSN.low)); + return true; +} + +bool rtps::deserializeMessage(const MessageProcessingInfo &info, SubmessageHeartbeat &msg) { + if (info.getRemainingSize() < SubmessageHeartbeat::getRawSize()) { + return false; + } + if (!deserializeMessage(info, msg.header)) { + return false; + } + + const uint8_t *currentPos = info.getPointerToCurrentPos() + SubmessageHeader::getRawSize(); + + doCopyAndMoveOn(msg.readerId.entityKey.data(), currentPos, msg.readerId.entityKey.size()); + msg.readerId.entityKind = static_cast(*currentPos++); + doCopyAndMoveOn(msg.writerId.entityKey.data(), currentPos, msg.writerId.entityKey.size()); + msg.writerId.entityKind = static_cast(*currentPos++); + doCopyAndMoveOn(reinterpret_cast(&msg.firstSN.high), currentPos, + sizeof(msg.firstSN.high)); + doCopyAndMoveOn(reinterpret_cast(&msg.firstSN.low), currentPos, + sizeof(msg.firstSN.low)); + doCopyAndMoveOn(reinterpret_cast(&msg.lastSN.high), currentPos, + sizeof(msg.lastSN.high)); + doCopyAndMoveOn(reinterpret_cast(&msg.lastSN.low), currentPos, sizeof(msg.lastSN.low)); + doCopyAndMoveOn(reinterpret_cast(&msg.count.value), currentPos, + sizeof(msg.count.value)); + return true; +} + +void rtps::deserializeSNS(const uint8_t *&position, SequenceNumberSet &set, + std::size_t num_bitfields) { + + doCopyAndMoveOn(reinterpret_cast(&set.base.high), position, + sizeof(SequenceNumber_t::high)); + doCopyAndMoveOn(reinterpret_cast(&set.base.low), position, + sizeof(SequenceNumber_t::low)); + doCopyAndMoveOn(reinterpret_cast(&set.numBits), position, sizeof(uint32_t)); + + // Ensure that we copy not more bits than our sequence number set can hold + if (set.numBits != 0) { + // equal to size = std::min(SNS_NUM_BYTES, num_bitfields) + std::size_t size = num_bitfields > SNS_NUM_BYTES ? SNS_NUM_BYTES : num_bitfields; + doCopyAndMoveOn(reinterpret_cast(set.bitMap.data()), position, size); + position += (num_bitfields - size); + } +} + +bool rtps::deserializeMessage(const MessageProcessingInfo &info, SubmessageAckNack &msg) { + const DataSize_t remainingSizeAtBeginning = info.getRemainingSize(); + if (remainingSizeAtBeginning < + SubmessageAckNack::getRawSizeWithoutSNSet()) { // Size of SequenceNumberSet unknown + return false; + } + if (!deserializeMessage(info, msg.header)) { + return false; + } + + const uint8_t *currentPos = info.getPointerToCurrentPos() + SubmessageHeader::getRawSize(); + + doCopyAndMoveOn(msg.readerId.entityKey.data(), currentPos, msg.readerId.entityKey.size()); + msg.readerId.entityKind = static_cast(*currentPos++); + doCopyAndMoveOn(msg.writerId.entityKey.data(), currentPos, msg.writerId.entityKey.size()); + msg.writerId.entityKind = static_cast(*currentPos++); + + std::size_t num_bitfields = msg.header.octetsToNextHeader - 4 - 4 - 8 - 4 - 4; + deserializeSNS(currentPos, msg.readerSNState, num_bitfields); + + doCopyAndMoveOn(reinterpret_cast(&msg.count.value), currentPos, + sizeof(msg.count.value)); + return true; +} + +bool rtps::deserializeMessage(const MessageProcessingInfo &info, SubmessageGap &msg) { + + const DataSize_t remainingSizeAtBeginning = info.getRemainingSize(); + if (remainingSizeAtBeginning < + SubmessageGap::getRawSizeWithoutSNSet()) { // Size of SequenceNumberSet + // unknown + return false; + } + if (!deserializeMessage(info, msg.header)) { + return false; + } + if (msg.header.octetsToNextHeader < + (SubmessageGap::getRawSizeWithSingleElementSNSet() - SubmessageHeader::getRawSize())) { + return false; + } + const uint8_t *currentPos = info.getPointerToCurrentPos() + SubmessageHeader::getRawSize(); + + doCopyAndMoveOn(msg.readerId.entityKey.data(), currentPos, msg.readerId.entityKey.size()); + msg.readerId.entityKind = static_cast(*currentPos++); + doCopyAndMoveOn(msg.writerId.entityKey.data(), currentPos, msg.writerId.entityKey.size()); + msg.writerId.entityKind = static_cast(*currentPos++); + + doCopyAndMoveOn(reinterpret_cast(&msg.gapStart.high), currentPos, + sizeof(msg.gapStart.high)); + doCopyAndMoveOn(reinterpret_cast(&msg.gapStart.low), currentPos, + sizeof(msg.gapStart.low)); + + std::size_t num_bitfields = msg.header.octetsToNextHeader - 4 - 4 - 8 - 8 - 4; + deserializeSNS(currentPos, msg.gapList, num_bitfields); + + return true; +} diff --git a/components/rtps_embedded/src/rtps_participant.cpp b/components/rtps_embedded/src/rtps_participant.cpp new file mode 100644 index 0000000000..7047ccc9f7 --- /dev/null +++ b/components/rtps_embedded/src/rtps_participant.cpp @@ -0,0 +1,287 @@ +#include "rtps_participant.hpp" + +#include + +#include "rtps/entities/Domain.hpp" + +// Host-side interface auto-detection uses platform-specific enumeration APIs. +#if defined(ESP_PLATFORM) +// No enumeration: ESP targets must pass interface_address explicitly. +#elif defined(_WIN32) +#include +#include + +// iphlpapi.h (GetAdaptersAddresses) must follow winsock2.h; the blank line keeps +// clang-format's alphabetical include sort from reordering it ahead of winsock2. +#include +#else +#include +#include +#include +#endif + +namespace espp { + +RtpsParticipant::RtpsParticipant(const Config &config) + : BaseComponent("RtpsParticipant", config.log_level) + , config_(config) {} + +RtpsParticipant::~RtpsParticipant() { stop(); } + +bool RtpsParticipant::resolve_interface_address(std::array &ip_bytes) const { + std::string addr = config_.interface_address; + // Skip loopback / link-local candidates during auto-detection. Unused on ESP + // targets, which require an explicit interface_address. + [[maybe_unused]] const auto usable = [](const std::string &ip) { + return !ip.empty() && ip.rfind("127.", 0) != 0 && ip.rfind("169.254.", 0) != 0; + }; +#if defined(ESP_PLATFORM) + if (addr.empty()) { + logger_.error("interface_address must be set on ESP targets (e.g. from the netif IP)"); + return false; + } +#elif defined(_WIN32) + if (addr.empty()) { + // Auto-detect: first up, non-loopback IPv4 adapter (GetAdaptersAddresses). + ULONG size = 15000; + std::vector buffer(size); + auto *adapters = reinterpret_cast(buffer.data()); + const ULONG flags = GAA_FLAG_SKIP_ANYCAST | GAA_FLAG_SKIP_MULTICAST | GAA_FLAG_SKIP_DNS_SERVER; + ULONG ret = GetAdaptersAddresses(AF_INET, flags, nullptr, adapters, &size); + if (ret == ERROR_BUFFER_OVERFLOW) { + buffer.resize(size); + adapters = reinterpret_cast(buffer.data()); + ret = GetAdaptersAddresses(AF_INET, flags, nullptr, adapters, &size); + } + if (ret == NO_ERROR) { + for (auto *a = adapters; a != nullptr && addr.empty(); a = a->Next) { + if (a->OperStatus != IfOperStatusUp || a->IfType == IF_TYPE_SOFTWARE_LOOPBACK) { + continue; + } + for (auto *ua = a->FirstUnicastAddress; ua != nullptr; ua = ua->Next) { + const auto *sin = reinterpret_cast(ua->Address.lpSockaddr); + if (sin == nullptr || sin->sin_family != AF_INET) { + continue; + } + char buf[INET_ADDRSTRLEN] = {0}; + if (inet_ntop(AF_INET, &sin->sin_addr, buf, sizeof(buf)) == nullptr) { + continue; + } + if (usable(buf)) { + addr = buf; + break; + } + } + } + } + if (addr.empty()) { + logger_.error("Could not auto-detect a usable IPv4 interface; set interface_address"); + return false; + } + logger_.info("Auto-detected interface address {}", addr); + } +#else + if (addr.empty()) { + // Auto-detect: first non-loopback, non-link-local IPv4 interface. + struct ifaddrs *ifaddr = nullptr; + if (getifaddrs(&ifaddr) == 0) { + for (struct ifaddrs *ifa = ifaddr; ifa != nullptr; ifa = ifa->ifa_next) { + if (ifa->ifa_addr == nullptr || ifa->ifa_addr->sa_family != AF_INET) { + continue; + } + char buf[INET_ADDRSTRLEN] = {0}; + const auto *sin = reinterpret_cast(ifa->ifa_addr); + if (inet_ntop(AF_INET, &sin->sin_addr, buf, sizeof(buf)) == nullptr) { + continue; + } + if (usable(buf)) { + addr = buf; + break; + } + } + freeifaddrs(ifaddr); + } + if (addr.empty()) { + logger_.error("Could not auto-detect a usable IPv4 interface; set interface_address"); + return false; + } + logger_.info("Auto-detected interface address {}", addr); + } +#endif + unsigned a = 0, b = 0, c = 0, d = 0; + if (std::sscanf(addr.c_str(), "%u.%u.%u.%u", &a, &b, &c, &d) != 4 || a > 255 || b > 255 || + c > 255 || d > 255) { + logger_.error("Invalid interface_address '{}'", addr); + return false; + } + ip_bytes = {static_cast(a), static_cast(b), static_cast(c), + static_cast(d)}; + return true; +} + +bool RtpsParticipant::start() { + std::lock_guard lock(mutex_); + if (started_) { + logger_.warn("Already started"); + return false; + } + + std::array ip_bytes{}; + if (!resolve_interface_address(ip_bytes)) { + return false; + } + + domain_ = std::make_unique(ip_bytes); + + // Engine lifecycle: participants must be created before completeInit() + // starts the discovery machinery; endpoints are added after. + participant_ = domain_->createParticipant(); + if (participant_ == nullptr) { + logger_.error("Could not create the RTPS participant"); + domain_.reset(); + return false; + } + + if (config_.on_publisher_matched) { + participant_->registerOnNewPublisherMatchedCallback(&publisher_matched_trampoline, this); + } + if (config_.on_subscriber_matched) { + participant_->registerOnNewSubscriberMatchedCallback(&subscriber_matched_trampoline, this); + } + + if (!domain_->completeInit()) { + logger_.error("RTPS domain init failed"); + participant_ = nullptr; + domain_.reset(); + return false; + } + + started_ = true; + logger_.info("Started (interface {}.{}.{}.{})", ip_bytes[0], ip_bytes[1], ip_bytes[2], + ip_bytes[3]); + return true; +} + +void RtpsParticipant::stop() { + std::lock_guard lock(mutex_); + if (!started_) { + return; + } + started_ = false; + domain_->stop(); + // The engine owns the endpoint objects; drop our references before the + // domain (and with it every writer/reader and their callback registrations) + // goes away. + writers_.clear(); + participant_ = nullptr; + domain_.reset(); + reader_contexts_.clear(); + logger_.info("Stopped"); +} + +bool RtpsParticipant::add_writer(const WriterConfig &config) { + std::lock_guard lock(mutex_); + if (!started_) { + logger_.error("Cannot add writer '{}': not started", config.topic); + return false; + } + if (writers_.count(config.topic) != 0) { + logger_.error("Writer for topic '{}' already exists", config.topic); + return false; + } + rtps::Writer *writer = + domain_->createWriter(*participant_, config.topic.c_str(), config.type_name.c_str(), + config.reliability == Reliability::RELIABLE); + if (writer == nullptr) { + logger_.error("Engine could not create writer '{}' (pool exhausted or name too long)", + config.topic); + return false; + } + writers_[config.topic] = writer; + logger_.info("Added {} writer: topic='{}' type='{}'", + config.reliability == Reliability::RELIABLE ? "reliable" : "best-effort", + config.topic, config.type_name); + return true; +} + +bool RtpsParticipant::add_reader(const ReaderConfig &config) { + std::lock_guard lock(mutex_); + if (!started_) { + logger_.error("Cannot add reader '{}': not started", config.topic); + return false; + } + rtps::Reader *reader = + domain_->createReader(*participant_, config.topic.c_str(), config.type_name.c_str(), + config.reliability == Reliability::RELIABLE); + if (reader == nullptr) { + logger_.error("Engine could not create reader '{}' (pool exhausted or name too long)", + config.topic); + return false; + } + auto ctx = std::make_unique(); + ctx->self = this; + ctx->on_sample = config.on_sample; + if (config.on_sample) { + if (reader->registerCallback(&reader_trampoline, ctx.get()) == 0) { + logger_.error("Engine could not register the sample callback for '{}'", config.topic); + return false; + } + } + reader_contexts_.push_back(std::move(ctx)); + logger_.info("Added {} reader: topic='{}' type='{}'", + config.reliability == Reliability::RELIABLE ? "reliable" : "best-effort", + config.topic, config.type_name); + return true; +} + +bool RtpsParticipant::publish(std::string_view topic, std::span cdr_payload) { + std::lock_guard lock(mutex_); + if (!started_) { + logger_.error("Cannot publish: not started"); + return false; + } + auto it = writers_.find(std::string(topic)); + if (it == writers_.end()) { + logger_.error("No writer for topic '{}'", topic); + return false; + } + const auto *change = it->second->newChange(rtps::ChangeKind_t::ALIVE, cdr_payload.data(), + static_cast(cdr_payload.size())); + if (change == nullptr) { + logger_.warn("Writer history full for topic '{}'; sample dropped", topic); + return false; + } + return true; +} + +void RtpsParticipant::reader_trampoline(void *arg, const rtps::ReaderCacheChange &change) { + auto *ctx = static_cast(arg); + if (ctx == nullptr || !ctx->on_sample) { + return; + } + // Serialize deliveries per reader: the engine may invoke this from a worker + // thread while a previous delivery is still running. + std::lock_guard lock(ctx->buffer_mutex); + const auto size = change.getDataSize(); + ctx->buffer.resize(size); + if (size == 0 || !change.copyInto(ctx->buffer.data(), size)) { + return; + } + ctx->on_sample(std::span(ctx->buffer.data(), ctx->buffer.size())); +} + +void RtpsParticipant::publisher_matched_trampoline(void *arg) { + auto *self = static_cast(arg); + if (self != nullptr && self->config_.on_publisher_matched) { + self->config_.on_publisher_matched(); + } +} + +void RtpsParticipant::subscriber_matched_trampoline(void *arg) { + auto *self = static_cast(arg); + if (self != nullptr && self->config_.on_subscriber_matched) { + self->config_.on_subscriber_matched(); + } +} + +} // namespace espp diff --git a/components/rtps_embedded/src/utils/Diagnostics.cpp b/components/rtps_embedded/src/utils/Diagnostics.cpp new file mode 100644 index 0000000000..84eb2c48b7 --- /dev/null +++ b/components/rtps_embedded/src/utils/Diagnostics.cpp @@ -0,0 +1,53 @@ +#include + +namespace rtps { +namespace Diagnostics { + +namespace ThreadPool { +uint32_t dropped_incoming_packets_usertraffic = 0; +uint32_t dropped_incoming_packets_metatraffic = 0; + +uint32_t dropped_outgoing_packets_usertraffic = 0; +uint32_t dropped_outgoing_packets_metatraffic = 0; + +uint32_t processed_incoming_metatraffic = 0; +uint32_t processed_outgoing_metatraffic = 0; +uint32_t processed_incoming_usertraffic = 0; +uint32_t processed_outgoing_usertraffic = 0; + +uint32_t max_ever_elements_outgoing_usertraffic_queue; +uint32_t max_ever_elements_incoming_usertraffic_queue; + +uint32_t max_ever_elements_outgoing_metatraffic_queue; +uint32_t max_ever_elements_incoming_metatraffic_queue; + +} // namespace ThreadPool + +namespace StatefulReader { +uint32_t sfr_unexpected_sn; +uint32_t sfr_retransmit_requests; +} // namespace StatefulReader + +namespace Network { +uint32_t lwip_allocation_failures; +} + +namespace SEDP { +uint32_t max_ever_remote_participants; +uint32_t current_remote_participants; + +uint32_t max_ever_matched_reader_proxies; +uint32_t current_max_matched_reader_proxies; + +uint32_t max_ever_matched_writer_proxies; +uint32_t current_max_matched_writer_proxies; + +uint32_t max_ever_unmatched_reader_proxies; +uint32_t current_max_unmatched_reader_proxies; + +uint32_t max_ever_unmatched_writer_proxies; +uint32_t current_max_unmatched_writer_proxies; +} // namespace SEDP + +} // namespace Diagnostics +} // namespace rtps diff --git a/components/socket/include/socket.hpp b/components/socket/include/socket.hpp index 864082ea83..b350430a25 100644 --- a/components/socket/include/socket.hpp +++ b/components/socket/include/socket.hpp @@ -184,6 +184,17 @@ class Socket : public BaseComponent { */ bool enable_reuse(); + /** + * @brief Disallow sharing this address/port combination. + * espp sockets enable address/port reuse at creation (see Socket::init), so a + * second bind of an in-use port silently succeeds. Call this before bind() + * when a conflict must fail loudly instead - e.g. RTPS unicast ports, where + * each participant needs a unique port and probes the next candidate on bind + * failure. + * @return true if SO_REUSEADDR and SO_REUSEPORT were successfully cleared. + */ + bool disable_reuse(); + /** * @brief Configure the socket to be multicast (if time_to_live > 0). * Sets the IP_MULTICAST_TTL (number of multicast hops allowed) and diff --git a/components/socket/src/socket.cpp b/components/socket/src/socket.cpp index 660679950c..71d9abfbdd 100644 --- a/components/socket/src/socket.cpp +++ b/components/socket/src/socket.cpp @@ -162,6 +162,31 @@ bool Socket::set_receive_timeout(const std::chrono::duration &timeout) { return true; } +bool Socket::disable_reuse() { +#if !CONFIG_LWIP_SO_REUSE && defined(ESP_PLATFORM) + // reuse is not compiled into lwip, so it is already effectively disabled + return true; +#else // CONFIG_LWIP_SO_REUSE || !defined(ESP_PLATFORM) + int err = 0; + int disabled = 0; + err = setsockopt(socket_, SOL_SOCKET, SO_REUSEADDR, reinterpret_cast(&disabled), + sizeof(disabled)); + if (err < 0) { + fmt::print(fg(fmt::color::red), "Couldn't clear SO_REUSEADDR: {}\n", error_string()); + return false; + } +#if !defined(ESP_PLATFORM) && !defined(_MSC_VER) + err = setsockopt(socket_, SOL_SOCKET, SO_REUSEPORT, reinterpret_cast(&disabled), + sizeof(disabled)); + if (err < 0) { + fmt::print(fg(fmt::color::red), "Couldn't clear SO_REUSEPORT: {}\n", error_string()); + return false; + } +#endif // !defined(ESP_PLATFORM) && !defined(_MSC_VER) + return true; +#endif // !CONFIG_LWIP_SO_REUSE && defined(ESP_PLATFORM) +} + bool Socket::enable_reuse() { #if !CONFIG_LWIP_SO_REUSE && defined(ESP_PLATFORM) fmt::print(fg(fmt::color::red), "CONFIG_LWIP_SO_REUSE not defined!\n"); diff --git a/lib/espp.cmake b/lib/espp.cmake index 868a7c4090..e66e38d777 100644 --- a/lib/espp.cmake +++ b/lib/espp.cmake @@ -36,7 +36,7 @@ set(ESPP_INCLUDES ${ESPP_COMPONENTS}/math/include ${ESPP_COMPONENTS}/ndef/include ${ESPP_COMPONENTS}/pid/include - ${ESPP_COMPONENTS}/rtps/include + ${ESPP_COMPONENTS}/rtps_embedded/include ${ESPP_COMPONENTS}/rtsp/include ${ESPP_COMPONENTS}/serialization/include ${ESPP_COMPONENTS}/tabulate/include @@ -59,7 +59,20 @@ 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/src/rtps.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/StatelessReader.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}/rtsp/src/rtcp_packet.cpp ${ESPP_COMPONENTS}/rtsp/src/rtp_packet.cpp ${ESPP_COMPONENTS}/rtsp/src/rtsp_client.cpp @@ -93,13 +106,14 @@ if(MSVC) list(APPEND ESPP_SOURCES ${CMAKE_CURRENT_LIST_DIR}/wcswidth.c) endif() -# On Windows link against ws2_32 (sockets) and winmm (timeBeginPeriod, used by -# the TimerResolution helper in espp.hpp). Centralizing these here keeps linkage -# consistent across the static library, tests, and the _espp module, and works -# on all Windows toolchains (MSVC, MinGW, clang) rather than relying on +# On Windows link against ws2_32 (sockets), winmm (timeBeginPeriod, used by the +# TimerResolution helper in espp.hpp), and iphlpapi (GetAdaptersAddresses, used +# by RtpsParticipant interface auto-detection). Centralizing these here keeps +# linkage consistent across the static library, tests, and the _espp module, and +# works on all Windows toolchains (MSVC, MinGW, clang) rather than relying on # MSVC-only #pragma comment(lib, ...). if(WIN32) - set(ESPP_EXTERNAL_LIBS ws2_32 winmm) + set(ESPP_EXTERNAL_LIBS ws2_32 winmm iphlpapi) else() set(ESPP_EXTERNAL_LIBS pthread) endif() diff --git a/lib/include/espp.hpp b/lib/include/espp.hpp index 6c38db58b2..541f4bf358 100644 --- a/lib/include/espp.hpp +++ b/lib/include/espp.hpp @@ -47,7 +47,7 @@ extern "C" { #include "rtp_depacketizer.hpp" #include "rtp_packetizer.hpp" #include "rtp_types.hpp" -#include "rtps.hpp" +#include "rtps_participant.hpp" #include "rtsp_client.hpp" #include "rtsp_server.hpp" #include "serialization.hpp" diff --git a/lib/python_bindings/rtps_bindings.cpp b/lib/python_bindings/rtps_bindings.cpp index be10e5b8ae..ccc1a4029d 100644 --- a/lib/python_bindings/rtps_bindings.cpp +++ b/lib/python_bindings/rtps_bindings.cpp @@ -1,28 +1,28 @@ -// Hand-written pybind11 bindings for the `rtps` component (RtpsParticipant). +// Hand-written pybind11 bindings for espp::RtpsParticipant (the facade over the +// embeddedRTPS engine in components/rtps_embedded — see its REFACTOR_PLAN.md). // -// Why hand-written (like cdr): RtpsParticipant exposes std::function callbacks (some taking -// std::span, which has no pybind caster), std::span publish/on_sample APIs, and a -// large nest of helper structs. litgen/srcmlcpp cannot bind these usefully. This shim exposes a -// clean, GIL-correct Python API: -// - publish(topic, bytes) / ReaderConfig.on_sample = callable(bytes) -// - discovery callbacks delivering ParticipantProxy / EndpointProxy objects -// - participant lifecycle + discovery queries +// Why hand-written (like cdr): the participant exposes std::function callbacks +// taking std::span (no pybind caster) and is invoked from engine +// background threads, so callbacks must be wrapped GIL-correctly. This shim +// exposes a clean Python API: +// - RtpsParticipant(Config(interface_address=..., ...)) +// - add_writer(topic=..., type_name=..., reliable=...) +// - add_reader(topic=..., type_name=..., reliable=..., on_sample=callable(bytes)) +// - publish(topic, bytes) // // It is kept out of the generated pybind_espp.cpp so regeneration never clobbers it. -#include -#include #include #include #include #include #include -#include +#include #include #include -#include "rtps.hpp" +#include "rtps_participant.hpp" namespace py = pybind11; using Rtps = espp::RtpsParticipant; @@ -33,236 +33,147 @@ py::bytes to_bytes(std::span s) { return py::bytes(reinterpret_cast(s.data()), s.size()); } -std::vector bytes_to_vec(const py::bytes &data) { - std::string s = data; - return std::vector(s.begin(), s.end()); +// Wrap a Python callable into a C++ std::function that the engine may copy and +// invoke from background threads that do not hold the GIL. Capturing the +// py::function directly would inc_ref without the GIL (a crash); a shared_ptr +// keeps copies GIL-free, and the callable is invoked / destroyed under the GIL. +// shared_ptr deleter that reacquires the GIL: the engine destroys its copies of +// these std::functions from background threads / under gil_scoped_release (e.g. +// in stop()), and destroying a py::function without the GIL aborts. +inline std::shared_ptr make_gil_safe_holder(const py::function &fn) { + return std::shared_ptr(new py::function(fn), [](py::function *p) { + py::gil_scoped_acquire gil; + delete p; + }); } -// Wrap a Python callable into a C++ std::function that is safe for the rtps component to copy and -// invoke from its background (receive / discovery) threads. The rtps component copies these -// std::functions on threads that do not hold the GIL; capturing the py::function directly would -// inc_ref the Python object without the GIL (a crash). Capturing a shared_ptr instead keeps the -// std::function copies GIL-free, and the callable is invoked / destroyed only under the GIL. -template -std::function wrap_callback(const py::function &fn, - std::function to_py) { +Rtps::sample_callback_t wrap_sample_callback(const py::function &fn) { if (!fn) { return {}; } - auto cb = std::make_shared(fn); - return [cb, to_py = std::move(to_py)](Arg arg) { + auto cb = make_gil_safe_holder(fn); + return [cb](std::span payload) { py::gil_scoped_acquire gil; - (*cb)(to_py(arg)); + try { + (*cb)(to_bytes(payload)); + } catch (py::error_already_set &e) { + e.discard_as_unraisable("RtpsParticipant on_sample"); + } }; } -// Reader config exposed to Python: like Rtps::ReaderConfig but `on_sample` is a Python callable -// taking `bytes` (the raw CDR sample). add_reader() adapts it into the span-based C++ callback. -struct PyReaderConfig { - std::string topic_name{}; - std::string type_name{"std_msgs/msg/UInt32"}; - Rtps::ReliabilityKind reliability{Rtps::ReliabilityKind::BEST_EFFORT}; - std::string multicast_group{}; - uint32_t entity_index{0}; - py::function on_sample{}; -}; - -Rtps::ReaderConfig to_reader_config(const PyReaderConfig &pc) { - Rtps::ReaderConfig rc; - rc.topic_name = pc.topic_name; - rc.type_name = pc.type_name; - rc.reliability = pc.reliability; - rc.multicast_group = pc.multicast_group; - rc.entity_index = pc.entity_index; - rc.on_sample = wrap_callback>( - pc.on_sample, [](std::span data) -> py::object { return to_bytes(data); }); - return rc; +Rtps::matched_callback_t wrap_matched_callback(const py::function &fn) { + if (!fn) { + return {}; + } + auto cb = make_gil_safe_holder(fn); + return [cb]() { + py::gil_scoped_acquire gil; + try { + (*cb)(); + } catch (py::error_already_set &e) { + e.discard_as_unraisable("RtpsParticipant matched callback"); + } + }; } -// Participant config exposed to Python. The discovery callbacks are Python callables (adapted the -// same GIL-safe way as on_sample). Task configs are left at their espp defaults. +// Python-facing Config: like Rtps::Config but with py::function callbacks. struct PyRtpsConfig { - std::string node_name{"espp_rtps"}; - uint16_t domain_id{0}; - uint16_t participant_id{0}; - std::string bind_address{"0.0.0.0"}; - std::string advertised_address{"127.0.0.1"}; - std::string metatraffic_multicast_group{"239.255.0.1"}; - std::string user_multicast_group{"239.255.0.1"}; - bool use_multicast_for_user_data{false}; - std::chrono::milliseconds announce_period{1000}; - std::string enclave{"/"}; - py::function on_participant_discovered{}; - py::function on_endpoint_discovered{}; - espp::Logger::Verbosity log_level{espp::Logger::Verbosity::INFO}; - espp::Logger::Verbosity socket_log_level{espp::Logger::Verbosity::WARN}; + std::string interface_address{}; + py::function on_publisher_matched{}; + py::function on_subscriber_matched{}; + espp::Logger::Verbosity log_level{espp::Logger::Verbosity::WARN}; }; Rtps::Config to_config(const PyRtpsConfig &pc) { - Rtps::Config c; - c.node_name = pc.node_name; - c.domain_id = pc.domain_id; - c.participant_id = pc.participant_id; - c.bind_address = pc.bind_address; - c.advertised_address = pc.advertised_address; - c.metatraffic_multicast_group = pc.metatraffic_multicast_group; - c.user_multicast_group = pc.user_multicast_group; - c.use_multicast_for_user_data = pc.use_multicast_for_user_data; - c.announce_period = pc.announce_period; - c.enclave = pc.enclave; - c.log_level = pc.log_level; - c.socket_log_level = pc.socket_log_level; - c.on_participant_discovered = wrap_callback( - pc.on_participant_discovered, - [](const Rtps::ParticipantProxy &p) -> py::object { return py::cast(p); }); - c.on_endpoint_discovered = wrap_callback( - pc.on_endpoint_discovered, - [](const Rtps::EndpointProxy &e) -> py::object { return py::cast(e); }); - return c; + return Rtps::Config{ + .interface_address = pc.interface_address, + .on_publisher_matched = wrap_matched_callback(pc.on_publisher_matched), + .on_subscriber_matched = wrap_matched_callback(pc.on_subscriber_matched), + .log_level = pc.log_level, + }; +} + +py::function as_function(const py::object &obj) { + if (obj.is_none()) { + return py::function{}; + } + return obj.cast(); } } // namespace void py_init_rtps(py::module &m) { - auto rtps = py::class_(m, "RtpsParticipant", py::dynamic_attr(), - "Cross-platform RTPS protocol participant (discovery + best-effort " - "CDR-over-RTPS user data)."); - - py::enum_(rtps, "ReliabilityKind") - .value("BEST_EFFORT", Rtps::ReliabilityKind::BEST_EFFORT) - .value("RELIABLE", Rtps::ReliabilityKind::RELIABLE); + auto rtps = py::class_( + m, "RtpsParticipant", + "RTPS/DDS participant (embeddedRTPS engine) for pub/sub interop with FastDDS and ROS 2.\n" + "Payloads are CDR-encapsulated bytes (see the cdr component / struct.pack).\n" + "For ROS 2 use topic 'rt/' and type '::msg::dds_::_'."); - py::class_(rtps, "GuidPrefix") - .def(py::init<>()) - .def_readonly("value", &Rtps::GuidPrefix::value) - .def("to_string", &Rtps::GuidPrefix::to_string) - .def("__repr__", &Rtps::GuidPrefix::to_string); + py::enum_(rtps, "Reliability") + .value("BEST_EFFORT", Rtps::Reliability::BEST_EFFORT) + .value("RELIABLE", Rtps::Reliability::RELIABLE); - py::class_(rtps, "EntityId") - .def(py::init<>()) - .def_readonly("value", &Rtps::EntityId::value) - .def("to_string", &Rtps::EntityId::to_string) - .def("__repr__", &Rtps::EntityId::to_string); - - py::class_(rtps, "Guid") - .def(py::init<>()) - .def_readonly("prefix", &Rtps::Guid::prefix) - .def_readonly("entity_id", &Rtps::Guid::entity_id) - .def("to_string", &Rtps::Guid::to_string) - .def("__repr__", &Rtps::Guid::to_string); - - auto locator = py::class_(rtps, "Locator"); - py::enum_(locator, "Kind") - .value("INVALID", Rtps::Locator::Kind::INVALID) - .value("UDP_V4", Rtps::Locator::Kind::UDP_V4); - locator.def(py::init<>()) - .def_static("udp_v4", &Rtps::Locator::udp_v4, py::arg("ipv4_address"), py::arg("port")) - .def_readwrite("kind", &Rtps::Locator::kind) - .def_readwrite("port", &Rtps::Locator::port) - .def("address_string", &Rtps::Locator::address_string); - - py::class_(rtps, "PortMapping") - .def(py::init<>()) - .def_readwrite("metatraffic_multicast", &Rtps::PortMapping::metatraffic_multicast) - .def_readwrite("metatraffic_unicast", &Rtps::PortMapping::metatraffic_unicast) - .def_readwrite("user_multicast", &Rtps::PortMapping::user_multicast) - .def_readwrite("user_unicast", &Rtps::PortMapping::user_unicast); - - py::class_(rtps, "ParticipantProxy") - .def_readonly("participant_guid", &Rtps::ParticipantProxy::participant_guid) - .def_readonly("guid_prefix", &Rtps::ParticipantProxy::guid_prefix) - .def_readonly("name", &Rtps::ParticipantProxy::name) - .def_readonly("enclave", &Rtps::ParticipantProxy::enclave) - .def_readonly("address", &Rtps::ParticipantProxy::address) - .def_readonly("ports", &Rtps::ParticipantProxy::ports) - .def_readonly("builtin_endpoints", &Rtps::ParticipantProxy::builtin_endpoints); - - py::class_(rtps, "EndpointProxy") - .def_readonly("guid", &Rtps::EndpointProxy::guid) - .def_readonly("participant_guid", &Rtps::EndpointProxy::participant_guid) - .def_readonly("topic_name", &Rtps::EndpointProxy::topic_name) - .def_readonly("type_name", &Rtps::EndpointProxy::type_name) - .def_readonly("reliability", &Rtps::EndpointProxy::reliability) - .def_readonly("is_reader", &Rtps::EndpointProxy::is_reader) - .def_readonly("expects_inline_qos", &Rtps::EndpointProxy::expects_inline_qos) - .def_readonly("unicast_locator", &Rtps::EndpointProxy::unicast_locator) - .def_readonly("multicast_locators", &Rtps::EndpointProxy::multicast_locators); - - py::class_(rtps, "WriterConfig") - .def(py::init<>()) - .def_readwrite("topic_name", &Rtps::WriterConfig::topic_name) - .def_readwrite("type_name", &Rtps::WriterConfig::type_name) - .def_readwrite("reliability", &Rtps::WriterConfig::reliability) - .def_readwrite("multicast_group", &Rtps::WriterConfig::multicast_group) - .def_readwrite("entity_index", &Rtps::WriterConfig::entity_index); - - py::class_(rtps, "ReaderConfig") - .def(py::init<>()) - .def_readwrite("topic_name", &PyReaderConfig::topic_name) - .def_readwrite("type_name", &PyReaderConfig::type_name) - .def_readwrite("reliability", &PyReaderConfig::reliability) - .def_readwrite("multicast_group", &PyReaderConfig::multicast_group) - .def_readwrite("entity_index", &PyReaderConfig::entity_index) - .def_readwrite("on_sample", &PyReaderConfig::on_sample, - "Callable invoked with the raw CDR sample (bytes) on a matching topic."); - - // Config: host-relevant fields. The discovery callbacks are Python callables receiving bound - // proxy objects, adapted GIL-safely (see wrap_callback / PyRtpsConfig). py::class_(rtps, "Config") - .def(py::init<>()) - .def_readwrite("node_name", &PyRtpsConfig::node_name) - .def_readwrite("domain_id", &PyRtpsConfig::domain_id) - .def_readwrite("participant_id", &PyRtpsConfig::participant_id) - .def_readwrite("bind_address", &PyRtpsConfig::bind_address) - .def_readwrite("advertised_address", &PyRtpsConfig::advertised_address) - .def_readwrite("metatraffic_multicast_group", &PyRtpsConfig::metatraffic_multicast_group) - .def_readwrite("user_multicast_group", &PyRtpsConfig::user_multicast_group) - .def_readwrite("use_multicast_for_user_data", &PyRtpsConfig::use_multicast_for_user_data) - .def_readwrite("announce_period", &PyRtpsConfig::announce_period) - .def_readwrite("enclave", &PyRtpsConfig::enclave) - .def_readwrite("on_participant_discovered", &PyRtpsConfig::on_participant_discovered) - .def_readwrite("on_endpoint_discovered", &PyRtpsConfig::on_endpoint_discovered) - .def_readwrite("log_level", &PyRtpsConfig::log_level) - .def_readwrite("socket_log_level", &PyRtpsConfig::socket_log_level); - - rtps.def(py::init([](const PyRtpsConfig &pc) { return std::make_unique(to_config(pc)); }), - py::arg("config")) - .def("start", &Rtps::start, py::call_guard()) - .def("stop", &Rtps::stop, py::call_guard()) + .def(py::init([](std::string interface_address, const py::object &on_publisher_matched, + const py::object &on_subscriber_matched, espp::Logger::Verbosity log_level) { + PyRtpsConfig c; + c.interface_address = std::move(interface_address); + c.on_publisher_matched = as_function(on_publisher_matched); + c.on_subscriber_matched = as_function(on_subscriber_matched); + c.log_level = log_level; + return c; + }), + py::arg("interface_address") = std::string{}, + py::arg("on_publisher_matched") = py::none(), + py::arg("on_subscriber_matched") = py::none(), + py::arg("log_level") = espp::Logger::Verbosity::WARN) + .def_readwrite("interface_address", &PyRtpsConfig::interface_address) + .def_readwrite("on_publisher_matched", &PyRtpsConfig::on_publisher_matched) + .def_readwrite("on_subscriber_matched", &PyRtpsConfig::on_subscriber_matched) + .def_readwrite("log_level", &PyRtpsConfig::log_level); + + rtps.def(py::init([](const PyRtpsConfig &config) { return new Rtps(to_config(config)); }), + py::arg("config") = PyRtpsConfig{}) + .def("start", &Rtps::start, py::call_guard(), + "Start the participant (transport + SPDP/SEDP discovery).") + .def("stop", &Rtps::stop, py::call_guard(), + "Stop the participant and its discovery/transport threads.") .def("is_started", &Rtps::is_started) - .def("add_writer", &Rtps::add_writer, py::arg("writer_config")) + .def( + "add_writer", + [](Rtps &self, const std::string &topic, const std::string &type_name, bool reliable) { + return self.add_writer({.topic = topic, + .type_name = type_name, + .reliability = reliable ? Rtps::Reliability::RELIABLE + : Rtps::Reliability::BEST_EFFORT}); + }, + py::arg("topic"), py::arg("type_name"), py::arg("reliable") = false, + py::call_guard(), "Add a publishing endpoint.") .def( "add_reader", - [](Rtps &self, const PyReaderConfig &rc) { - return self.add_reader(to_reader_config(rc)); + [](Rtps &self, const std::string &topic, const std::string &type_name, bool reliable, + const py::object &on_sample) { + // wrap under the GIL (we hold it here), then release for the engine call + auto cb = wrap_sample_callback(as_function(on_sample)); + py::gil_scoped_release release; + return self.add_reader({.topic = topic, + .type_name = type_name, + .reliability = reliable ? Rtps::Reliability::RELIABLE + : Rtps::Reliability::BEST_EFFORT, + .on_sample = std::move(cb)}); }, - py::arg("reader_config")) - .def("discovered_participants", &Rtps::discovered_participants) - .def("discovered_writers", &Rtps::discovered_writers) - .def("discovered_readers", &Rtps::discovered_readers) - .def("writers", &Rtps::writers) - .def("readers", - [](const Rtps &self) { - // Return the host-friendly reader view (without the C++ span callback). - std::vector out; - for (const auto &r : self.readers()) { - out.push_back({r.topic_name, r.type_name, r.reliability, r.multicast_group, - r.entity_index, py::function{}}); - } - return out; - }) - .def("ports", &Rtps::ports) - .def("participant_guid", &Rtps::participant_guid) - .def("writer_guid", &Rtps::writer_guid, py::arg("index")) - .def("reader_guid", &Rtps::reader_guid, py::arg("index")) + py::arg("topic"), py::arg("type_name"), py::arg("reliable") = false, + py::arg("on_sample") = py::none(), + "Add a subscribing endpoint; on_sample receives each sample as bytes.") .def( "publish", - [](Rtps &self, std::string_view topic, const py::bytes &cdr_payload) { - auto vec = bytes_to_vec(cdr_payload); + [](Rtps &self, const std::string &topic, const py::bytes &data) { + std::string s = data; + const std::vector payload(s.begin(), s.end()); py::gil_scoped_release release; - return self.publish(topic, std::span{vec.data(), vec.size()}); + return self.publish(topic, payload); }, - py::arg("topic_name"), py::arg("cdr_payload")) - .def_static("compute_port_mapping", &Rtps::compute_port_mapping, py::arg("domain_id"), - py::arg("participant_id")); + py::arg("topic"), py::arg("data"), + "Publish a CDR-encapsulated sample (bytes) on a topic added with add_writer()."); } diff --git a/pc/CMakeLists.txt b/pc/CMakeLists.txt index 69e7eee409..ef907e3d85 100644 --- a/pc/CMakeLists.txt +++ b/pc/CMakeLists.txt @@ -4,6 +4,7 @@ set(CMAKE_CXX_STANDARD 23) include(${CMAKE_CURRENT_SOURCE_DIR}/../lib/espp.cmake) + MACRO(GEN_TESTS curdir) # get test files FILE(GLOB tests RELATIVE ${curdir} ${curdir}/tests/*.cpp) @@ -20,6 +21,8 @@ MACRO(GEN_TESTS curdir) add_definitions(-D_CRT_SECURE_NO_WARNINGS) endif() add_executable(${TEST_NAME} ${test_file}) + + target_include_directories(${TEST_NAME} PRIVATE ${curdir}/../lib/pc/include) target_link_directories(${TEST_NAME} PRIVATE diff --git a/pc/tests/rtps_embedded_golden.cpp b/pc/tests/rtps_embedded_golden.cpp new file mode 100644 index 0000000000..637428748f --- /dev/null +++ b/pc/tests/rtps_embedded_golden.cpp @@ -0,0 +1,218 @@ +// Golden wire-format tests for the embeddedRTPS engine (components/rtps_embedded). +// +// Phase 0b of components/rtps_embedded/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, +// ACKNACK (multi-word MSB-first SequenceNumberSet), GAP, and the SEDP TopicData +// 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 +// +// Exits 0 when every section matches its golden byte string; 1 otherwise. + +#include +#include +#include +#include +#include + +#include "rtps/discovery/TopicData.hpp" +#include "rtps/messages/MessageFactory.hpp" +#include "rtps/storages/PayloadBuffer.hpp" +#include "rtps/utils/CdrBuffer.hpp" + +namespace { + +struct Golden { + const char *name; + std::span bytes; +}; + +// Fixed inputs shared by all sections +constexpr rtps::GuidPrefix_t kPrefix{ + {0x01, 0x02, 0x03, 0x04, 0x05, 0x06, 0x07, 0x08, 0x09, 0x0A, 0x0B, 0x0C}}; +const rtps::EntityId_t kWriterId{{0x00, 0x00, 0x01}, + rtps::EntityKind_t::USER_DEFINED_WRITER_WITHOUT_KEY}; +const rtps::EntityId_t kReaderId{{0x00, 0x00, 0x02}, + rtps::EntityKind_t::USER_DEFINED_READER_WITHOUT_KEY}; + +std::vector build_header() { + rtps::PayloadBuffer b; + rtps::MessageFactory::addHeader(b, kPrefix); + return b.bytes; +} + +std::vector build_info_dst() { + rtps::PayloadBuffer b; + rtps::GuidPrefix_t dst = kPrefix; + rtps::MessageFactory::addSubMessageInfoDST(b, dst); + return b.bytes; +} + +std::vector build_info_ts_invalid() { + rtps::PayloadBuffer b; + rtps::MessageFactory::addSubMessageTimeStamp(b, /*setInvalid=*/true); + return b.bytes; +} + +std::vector build_data() { + // CDR_LE-encapsulated "hello" string payload (4B encap + 4B length + 6B chars) + static constexpr uint8_t kPayload[] = {0x00, 0x01, 0x00, 0x00, 0x06, 0x00, 0x00, + 0x00, 'h', 'e', 'l', 'l', 'o', 0x00}; + rtps::PayloadBuffer payload; + payload.append(kPayload, sizeof(kPayload)); + rtps::PayloadBuffer b; + rtps::MessageFactory::addSubMessageData(b, payload, /*containsInlineQos=*/false, + rtps::SequenceNumber_t{0, 5}, kWriterId, kReaderId); + return b.bytes; +} + +std::vector build_heartbeat() { + rtps::PayloadBuffer b; + rtps::MessageFactory::addHeartbeat(b, kWriterId, kReaderId, rtps::SequenceNumber_t{0, 1}, + rtps::SequenceNumber_t{0, 7}, rtps::Count_t{3}); + return b.bytes; +} + +std::vector build_acknack() { + // Multi-word SequenceNumberSet: 40 bits, MSB-first bit order + // (bitMap[bucket] & 1 << (31 - pos)): bits 0 and 31 in word 0, bit 33 in word 1. + rtps::SequenceNumberSet sns; + sns.base = rtps::SequenceNumber_t{0, 4}; + sns.numBits = 40; + sns.bitMap[0] = 0x80000001; + sns.bitMap[1] = 0x40000000; + rtps::PayloadBuffer b; + rtps::MessageFactory::addAckNack(b, kWriterId, kReaderId, sns, rtps::Count_t{9}, + /*final_flag=*/false); + return b.bytes; +} + +std::vector build_gap() { + rtps::PayloadBuffer b; + rtps::MessageFactory::addSubmessageGap(b, kWriterId, kReaderId, rtps::SequenceNumber_t{0, 5}, + rtps::SequenceNumber_t{0, 9}); + return b.bytes; +} + +rtps::TopicData make_topic_data() { + rtps::TopicData td; + td.endpointGuid.prefix = kPrefix; + td.endpointGuid.entityId = kWriterId; + // Odd-length names exercise the CDR string length-prefix + null terminator + + // 4-byte parameter alignment corner cases. + std::strncpy(td.typeName, "std_msgs::msg::dds_::String_", sizeof(td.typeName)); + std::strncpy(td.topicName, "rt/chatter", sizeof(td.topicName)); + td.reliabilityKind = rtps::ReliabilityKind_t::RELIABLE; + td.durabilityKind = rtps::DurabilityKind_t::TRANSIENT_LOCAL; + td.unicastLocator = rtps::FullLengthLocator::createUDPv4Locator(192, 168, 1, 2, 7411); + td.multicastLocator = rtps::FullLengthLocator::createUDPv4Locator(239, 255, 0, 1, 7400); + return td; +} + +std::vector build_sedp_topic_data() { + const rtps::TopicData td = make_topic_data(); + std::vector buf(1024, 0); + rtps::CdrSink sink{rtps::asWritableBytes(buf.data(), buf.size())}; + rtps::CdrWriter writer(sink); + if (!td.serializeInto(writer)) { + return {}; + } + buf.resize(sink.size()); + return buf; +} + +bool roundtrip_sedp_topic_data() { + const rtps::TopicData td = make_topic_data(); + std::vector bytes = build_sedp_topic_data(); + if (bytes.empty()) { + std::printf("FAIL: sedp serialize returned no bytes\n"); + return false; + } + rtps::TopicData parsed; + if (!parsed.readFromBuffer(std::span(bytes.data(), bytes.size()))) { + std::printf("FAIL: sedp round-trip parse failed\n"); + return false; + } + if (std::strcmp(parsed.topicName, td.topicName) != 0 || + std::strcmp(parsed.typeName, td.typeName) != 0 || !(parsed.endpointGuid == td.endpointGuid) || + parsed.reliabilityKind != td.reliabilityKind) { + std::printf("FAIL: sedp round-trip field mismatch\n"); + return false; + } + return true; +} + +void dump_array(const char *name, const std::vector &bytes) { + std::printf("static constexpr uint8_t kGolden_%s[] = {", name); + for (size_t i = 0; i < bytes.size(); i++) { + if (i % 12 == 0) { + std::printf("\n "); + } + std::printf("0x%02X,%s", bytes[i], (i + 1 < bytes.size()) ? " " : ""); + } + std::printf("};\n"); +} + +bool check(const char *name, const std::vector &actual, std::span golden) { + if (actual.size() == golden.size() && + std::memcmp(actual.data(), golden.data(), golden.size()) == 0) { + std::printf("PASS: %-16s (%zu bytes)\n", name, actual.size()); + return true; + } + std::printf("FAIL: %-16s actual %zu bytes vs golden %zu bytes\n", name, actual.size(), + golden.size()); + const size_t n = std::min(actual.size(), golden.size()); + for (size_t i = 0; i < n; i++) { + if (actual[i] != golden[i]) { + std::printf(" first diff at byte %zu: actual 0x%02X vs golden 0x%02X\n", i, actual[i], + golden[i]); + break; + } + } + return false; +} + +// Golden byte strings captured from the current (interop-proven) implementation. +#include "rtps_embedded_golden.inc" + +} // namespace + +int main(int argc, char **argv) { + struct Section { + const char *name; + std::vector (*build)(); + std::span golden; + }; + const Section sections[] = { + {"header", build_header, kGolden_header}, + {"info_dst", build_info_dst, kGolden_info_dst}, + {"info_ts_invalid", build_info_ts_invalid, kGolden_info_ts_invalid}, + {"data", build_data, kGolden_data}, + {"heartbeat", build_heartbeat, kGolden_heartbeat}, + {"acknack", build_acknack, kGolden_acknack}, + {"gap", build_gap, kGolden_gap}, + {"sedp_topic_data", build_sedp_topic_data, kGolden_sedp_topic_data}, + }; + + if (argc > 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("// Do NOT edit by hand; regenerate only on an intentional wire change.\n"); + for (const auto &s : sections) { + dump_array(s.name, s.build()); + } + return 0; + } + + bool ok = true; + for (const auto &s : sections) { + ok &= check(s.name, s.build(), s.golden); + } + ok &= roundtrip_sedp_topic_data(); + std::printf(ok ? "PASS\n" : "FAIL\n"); + return ok ? 0 : 1; +} diff --git a/pc/tests/rtps_embedded_golden.inc b/pc/tests/rtps_embedded_golden.inc new file mode 100644 index 0000000000..39ed0c84e4 --- /dev/null +++ b/pc/tests/rtps_embedded_golden.inc @@ -0,0 +1,45 @@ +// Golden wire-format byte strings for rtps_embedded_golden.cpp. +// Generated by: ./rtps_embedded_golden --dump > ../tests/rtps_embedded_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, + 0x05, 0x06, 0x07, 0x08, 0x09, 0x0A, 0x0B, 0x0C,}; +static constexpr uint8_t kGolden_info_dst[] = { + 0x0E, 0x01, 0x0C, 0x00, 0x01, 0x02, 0x03, 0x04, 0x05, 0x06, 0x07, 0x08, + 0x09, 0x0A, 0x0B, 0x0C,}; +static constexpr uint8_t kGolden_info_ts_invalid[] = { + 0x09, 0x03, 0x00, 0x00,}; +static constexpr uint8_t kGolden_data[] = { + 0x15, 0x05, 0x22, 0x00, 0x00, 0x00, 0x10, 0x00, 0x00, 0x00, 0x02, 0x04, + 0x00, 0x00, 0x01, 0x03, 0x00, 0x00, 0x00, 0x00, 0x05, 0x00, 0x00, 0x00, + 0x00, 0x01, 0x00, 0x00, 0x06, 0x00, 0x00, 0x00, 0x68, 0x65, 0x6C, 0x6C, + 0x6F, 0x00,}; +static constexpr uint8_t kGolden_heartbeat[] = { + 0x07, 0x01, 0x1C, 0x00, 0x00, 0x00, 0x02, 0x04, 0x00, 0x00, 0x01, 0x03, + 0x00, 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x07, 0x00, 0x00, 0x00, 0x03, 0x00, 0x00, 0x00,}; +static constexpr uint8_t kGolden_acknack[] = { + 0x06, 0x01, 0x20, 0x00, 0x00, 0x00, 0x02, 0x04, 0x00, 0x00, 0x01, 0x03, + 0x00, 0x00, 0x00, 0x00, 0x04, 0x00, 0x00, 0x00, 0x28, 0x00, 0x00, 0x00, + 0x01, 0x00, 0x00, 0x80, 0x00, 0x00, 0x00, 0x40, 0x09, 0x00, 0x00, 0x00,}; +static constexpr uint8_t kGolden_gap[] = { + 0x08, 0x01, 0x20, 0x00, 0x00, 0x00, 0x02, 0x04, 0x00, 0x00, 0x01, 0x03, + 0x00, 0x00, 0x00, 0x00, 0x05, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x09, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,}; +static constexpr uint8_t kGolden_sedp_topic_data[] = { + 0x2F, 0x00, 0x18, 0x00, 0x01, 0x00, 0x00, 0x00, 0xF3, 0x1C, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0xC0, 0xA8, 0x01, 0x02, 0x30, 0x00, 0x18, 0x00, 0x01, 0x00, 0x00, 0x00, + 0xE8, 0x1C, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0xEF, 0xFF, 0x00, 0x01, 0x05, 0x00, 0x10, 0x00, + 0x0B, 0x00, 0x00, 0x00, 0x72, 0x74, 0x2F, 0x63, 0x68, 0x61, 0x74, 0x74, + 0x65, 0x72, 0x00, 0x00, 0x07, 0x00, 0x24, 0x00, 0x1D, 0x00, 0x00, 0x00, + 0x73, 0x74, 0x64, 0x5F, 0x6D, 0x73, 0x67, 0x73, 0x3A, 0x3A, 0x6D, 0x73, + 0x67, 0x3A, 0x3A, 0x64, 0x64, 0x73, 0x5F, 0x3A, 0x3A, 0x53, 0x74, 0x72, + 0x69, 0x6E, 0x67, 0x5F, 0x00, 0x00, 0x00, 0x00, 0x70, 0x00, 0x10, 0x00, + 0x01, 0x02, 0x03, 0x04, 0x05, 0x06, 0x07, 0x08, 0x09, 0x0A, 0x0B, 0x0C, + 0x00, 0x00, 0x01, 0x03, 0x5A, 0x00, 0x10, 0x00, 0x01, 0x02, 0x03, 0x04, + 0x05, 0x06, 0x07, 0x08, 0x09, 0x0A, 0x0B, 0x0C, 0x00, 0x00, 0x01, 0x03, + 0x1A, 0x00, 0x0C, 0x00, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x1D, 0x00, 0x04, 0x00, 0x01, 0x00, 0x00, 0x00, + 0x01, 0x00, 0x00, 0x00,}; diff --git a/pc/tests/rtps_embedded_interop_pub.cpp b/pc/tests/rtps_embedded_interop_pub.cpp new file mode 100644 index 0000000000..7f8d5477c2 --- /dev/null +++ b/pc/tests/rtps_embedded_interop_pub.cpp @@ -0,0 +1,82 @@ +// RTPS interop publisher (Phase 1 of components/rtps_embedded/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] +// [interface_ip] Exits 0 after publishing `count` samples. + +#include +#include +#include +#include +#include + +#include "cdr.hpp" +#include "rtps_participant.hpp" + +// std_msgs/msg/String, serialized via the reflection-driven cdr component: +// cdr::serialize emits the ROS 2 / classic-CDR wire format +// (4-byte encapsulation header + CDR body) for any reflectable struct. +struct StringMsg { + std::string data; +}; + +// The cdr component works in std::byte; the facade publish/on_sample API uses +// uint8_t spans - bridge the two views (same bytes, different value type). +inline std::span u8_span(const std::vector &bytes) { + return {reinterpret_cast(bytes.data()), bytes.size()}; +} + +using namespace std::chrono_literals; + +int main(int argc, char **argv) { + const char *topic = (argc > 1) ? argv[1] : "rt/chatter"; + const char *type = (argc > 2) ? argv[2] : "std_msgs::msg::dds_::String_"; + const bool reliable = (argc > 3) ? (std::atoi(argv[3]) != 0) : true; + const int count = (argc > 4) ? std::atoi(argv[4]) : 30; + const int period_ms = (argc > 5) ? std::atoi(argv[5]) : 200; + const char *interface_ip = (argc > 6) ? argv[6] : ""; // "" -> auto-detect + + espp::RtpsParticipant participant({ + .interface_address = interface_ip, + .log_level = espp::Logger::Verbosity::INFO, + }); + if (!participant.start()) { + std::printf("FAIL: start\n"); + return 1; + } + using Reliability = espp::RtpsParticipant::Reliability; + if (!participant.add_writer({ + .topic = topic, + .type_name = type, + .reliability = reliable ? Reliability::RELIABLE : Reliability::BEST_EFFORT, + })) { + std::printf("FAIL: add_writer\n"); + return 1; + } + std::printf("interop_pub: topic=%s type=%s reliable=%d count=%d\n", topic, type, reliable ? 1 : 0, + count); + + // Give SPDP/SEDP a moment to match before the first sample. + std::this_thread::sleep_for(2s); + + int sent = 0; + for (int i = 0; i < count; i++) { + auto bytes = cdr::serialize(StringMsg{"espp interop " + std::to_string(i)}); + if (bytes && participant.publish(topic, u8_span(*bytes))) { + sent++; + std::printf("sent %d\n", sent); + std::fflush(stdout); + } + std::this_thread::sleep_for(std::chrono::milliseconds(period_ms)); + } + + // Let reliable retransmits drain before tearing down. + std::this_thread::sleep_for(1s); + std::printf("DONE sent=%d\n", sent); + participant.stop(); + return sent == count ? 0 : 1; +} diff --git a/pc/tests/rtps_embedded_interop_sub.cpp b/pc/tests/rtps_embedded_interop_sub.cpp new file mode 100644 index 0000000000..fd284c9b88 --- /dev/null +++ b/pc/tests/rtps_embedded_interop_sub.cpp @@ -0,0 +1,85 @@ +// RTPS interop subscriber (Phase 1 of components/rtps_embedded/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] +// [interface_ip] Exits 0 once `required` samples arrive within `timeout_s`. + +#include +#include +#include +#include +#include +#include + +#include "cdr.hpp" +#include "rtps_participant.hpp" + +// std_msgs/msg/String, serialized via the reflection-driven cdr component: +// cdr::serialize emits the ROS 2 / classic-CDR wire format +// (4-byte encapsulation header + CDR body) for any reflectable struct. +struct StringMsg { + std::string data; +}; + +// The cdr component works in std::byte; the facade publish/on_sample API uses +// uint8_t spans - bridge the two views (same bytes, different value type). +inline std::span u8_span(const std::vector &bytes) { + return {reinterpret_cast(bytes.data()), bytes.size()}; +} + +using namespace std::chrono_literals; + +int main(int argc, char **argv) { + const char *topic = (argc > 1) ? argv[1] : "rt/chatter"; + const char *type = (argc > 2) ? argv[2] : "std_msgs::msg::dds_::String_"; + const bool reliable = (argc > 3) ? (std::atoi(argv[3]) != 0) : true; + const int required = (argc > 4) ? std::atoi(argv[4]) : 5; + const int timeout_s = (argc > 5) ? std::atoi(argv[5]) : 30; + const char *interface_ip = (argc > 6) ? argv[6] : ""; // "" -> auto-detect + + std::atomic received{0}; + + espp::RtpsParticipant participant({ + .interface_address = interface_ip, + .log_level = espp::Logger::Verbosity::INFO, + }); + if (!participant.start()) { + std::printf("FAIL: start\n"); + return 1; + } + using Reliability = espp::RtpsParticipant::Reliability; + if (!participant.add_reader({ + .topic = topic, + .type_name = type, + .reliability = reliable ? Reliability::RELIABLE : Reliability::BEST_EFFORT, + .on_sample = + [&received](std::span cdr_payload) { + auto msg = cdr::deserialize(std::as_bytes(cdr_payload)); + if (msg) { + const int n = received.fetch_add(1) + 1; + std::printf("received %d: '%s'\n", n, msg->data.c_str()); + std::fflush(stdout); + } + }, + })) { + std::printf("FAIL: add_reader\n"); + return 1; + } + std::printf("interop_sub: topic=%s type=%s reliable=%d required=%d timeout=%ds\n", topic, type, + reliable ? 1 : 0, required, timeout_s); + + const auto start = std::chrono::steady_clock::now(); + while (received.load() < required && + std::chrono::steady_clock::now() - start < std::chrono::seconds(timeout_s)) { + std::this_thread::sleep_for(100ms); + } + + const int n = received.load(); + std::printf("%s received=%d required=%d\n", n >= required ? "PASS" : "FAIL", n, required); + participant.stop(); + return n >= required ? 0 : 1; +} diff --git a/pc/tests/rtps_embedded_pubsub.cpp b/pc/tests/rtps_embedded_pubsub.cpp new file mode 100644 index 0000000000..f4315d1e63 --- /dev/null +++ b/pc/tests/rtps_embedded_pubsub.cpp @@ -0,0 +1,128 @@ +// Host loopback pub/sub test for the embeddedRTPS engine (components/rtps_embedded). +// +// Phase 0a of components/rtps_embedded/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 +// note at the createWriter call). +// +// Exits 0 when at least kRequiredSamples samples arrive within the deadline; 1 otherwise. + +#include +#include +#include +#include +#include +#include +#include + +#include "rtps/entities/Domain.hpp" +#include "rtps/utils/CdrBuffer.hpp" + +#include "rtps_common.hpp" + +using namespace std::chrono_literals; + +namespace { +constexpr int kRequiredSamples = 5; +constexpr auto kDeadline = 20s; +constexpr const char *kTopic = "espp_loopback"; +constexpr const char *kType = "std_msgs::msg::String"; + +std::atomic g_received{0}; + +void reader_cb(void * /*callee*/, const rtps::ReaderCacheChange &change) { + // 4-byte CDR encapsulation header + at least a string length prefix + if (change.getDataSize() < 8) { + return; + } + g_received.fetch_add(1); +} + +bool publish_string(rtps::Writer *writer, const char *text) { + uint8_t cdr_buf[4 + 4 + 128]; + cdr_buf[0] = 0x00; // CDR_LE encapsulation + cdr_buf[1] = 0x01; + cdr_buf[2] = 0x00; + cdr_buf[3] = 0x00; + // CDR string body: uint32 length (incl. null terminator) + chars + null. + rtps::CdrSink sink{rtps::asWritableBytes(cdr_buf + 4, sizeof(cdr_buf) - 4)}; + rtps::CdrWriter writer_cdr(sink); + const auto len = static_cast(std::strlen(text) + 1); + writer_cdr.write(len); + rtps::writeBytes(writer_cdr, reinterpret_cast(text), len); + if (!writer_cdr.ok()) { + return false; + } + const auto total = static_cast(4 + sink.size()); + return writer->newChange(rtps::ChangeKind_t::ALIVE, cdr_buf, total) != nullptr; +} +} // namespace + +int main() { + const std::string ip_str = rtps_test::guess_local_ipv4(); + rtps::Ip4AddressBytes ip{}; + unsigned a = 0, b = 0, c = 0, d = 0; + if (std::sscanf(ip_str.c_str(), "%u.%u.%u.%u", &a, &b, &c, &d) != 4) { + std::printf("FAIL: could not parse local ip '%s'\n", ip_str.c_str()); + return 1; + } + ip = {static_cast(a), static_cast(b), static_cast(c), + static_cast(d)}; + std::printf("local ip: %s\n", ip_str.c_str()); + + static rtps::Domain domain(ip); + + // Participants must exist before completeInit() starts discovery. + rtps::Participant *pub_part = domain.createParticipant(); + rtps::Participant *sub_part = domain.createParticipant(); + if (pub_part == nullptr || sub_part == nullptr) { + std::printf("FAIL: could not create participants\n"); + return 1; + } + + if (!domain.completeInit()) { + std::printf("FAIL: completeInit\n"); + return 1; + } + + // Best-effort on both ends: with MAX_NUM_PARTICIPANTS=2 the SEDP builtins consume + // the entire desktop stateful pools (2 participants x 2 SEDP writers = 4 = cap), so + // the loopback baseline uses the spare stateless slots. Reliable QoS is exercised + // against FastDDS in the Phase 0c interop harness (single local participant). + rtps::Writer *writer = domain.createWriter(*pub_part, kTopic, kType, /*reliable=*/false); + rtps::Reader *reader = domain.createReader(*sub_part, kTopic, kType, /*reliable=*/false); + if (writer == nullptr || reader == nullptr) { + std::printf("FAIL: could not create writer/reader\n"); + domain.stop(); + return 1; + } + if (reader->registerCallback(reader_cb, nullptr) == 0) { + std::printf("FAIL: registerCallback\n"); + domain.stop(); + return 1; + } + + const auto start = std::chrono::steady_clock::now(); + int sent = 0; + while (g_received.load() < kRequiredSamples && + std::chrono::steady_clock::now() - start < kDeadline) { + char text[64]; + std::snprintf(text, sizeof(text), "hello espp rtps %d", sent); + if (publish_string(writer, text)) { + sent++; + } + std::this_thread::sleep_for(100ms); + } + + const int received = g_received.load(); + std::printf("sent=%d received=%d\n", sent, received); + domain.stop(); + + if (received >= kRequiredSamples) { + std::printf("PASS\n"); + return 0; + } + std::printf("FAIL: received %d < %d\n", received, kRequiredSamples); + return 1; +} diff --git a/pc/tests/rtps_facade_pubsub.cpp b/pc/tests/rtps_facade_pubsub.cpp new file mode 100644 index 0000000000..6d9b0344af --- /dev/null +++ b/pc/tests/rtps_facade_pubsub.cpp @@ -0,0 +1,88 @@ +// In-process facade loopback: two espp::RtpsParticipant instances (two Domains) +// in one process discover each other and exchange samples. +// +// This exercises the Phase 2a unicast-port probing fix (REFACTOR_PLAN.md): the +// second participant's Domain finds the first's ports taken (bind with reuse +// disabled fails loudly) and probes forward to the next participant id. +// Before that fix, both Domains silently shared the same ports and could never +// discover each other. +// +// Exits 0 when at least kRequired samples arrive within the deadline. + +#include +#include +#include +#include +#include + +#include "cdr.hpp" +#include "rtps_participant.hpp" + +// std_msgs/msg/String, serialized via the reflection-driven cdr component: +// cdr::serialize emits the ROS 2 / classic-CDR wire format +// (4-byte encapsulation header + CDR body) for any reflectable struct. +struct StringMsg { + std::string data; +}; + +// The cdr component works in std::byte; the facade publish/on_sample API uses +// uint8_t spans - bridge the two views (same bytes, different value type). +inline std::span u8_span(const std::vector &bytes) { + return {reinterpret_cast(bytes.data()), bytes.size()}; +} + +using namespace std::chrono_literals; + +int main() { + constexpr int kRequired = 5; + constexpr auto kDeadline = 20s; + const char *topic = "facade_loopback"; + const char *type = "std_msgs::msg::dds_::String_"; + + std::atomic received{0}; + using Reliability = espp::RtpsParticipant::Reliability; + + espp::RtpsParticipant pub({.log_level = espp::Logger::Verbosity::INFO}); + espp::RtpsParticipant sub({.log_level = espp::Logger::Verbosity::INFO}); + + if (!pub.start() || !sub.start()) { + std::printf("FAIL: start\n"); + return 1; + } + if (!pub.add_writer({.topic = topic, .type_name = type, .reliability = Reliability::RELIABLE})) { + std::printf("FAIL: add_writer\n"); + return 1; + } + if (!sub.add_reader({.topic = topic, + .type_name = type, + .reliability = Reliability::RELIABLE, + .on_sample = [&received](std::span payload) { + if (cdr::deserialize(std::as_bytes(payload))) { + received.fetch_add(1); + } + }})) { + std::printf("FAIL: add_reader\n"); + return 1; + } + + int sent = 0; + const auto start = std::chrono::steady_clock::now(); + while (received.load() < kRequired && std::chrono::steady_clock::now() - start < kDeadline) { + auto bytes = cdr::serialize(StringMsg{"facade loopback " + std::to_string(sent)}); + if (bytes && pub.publish(topic, u8_span(*bytes))) { + sent++; + } + std::this_thread::sleep_for(100ms); + } + + const int n = received.load(); + std::printf("sent=%d received=%d\n", sent, n); + pub.stop(); + sub.stop(); + if (n >= kRequired) { + std::printf("PASS\n"); + return 0; + } + std::printf("FAIL\n"); + return 1; +} diff --git a/pc/tests/rtps_publisher.cpp b/pc/tests/rtps_publisher.cpp deleted file mode 100644 index a8ea31b01b..0000000000 --- a/pc/tests/rtps_publisher.cpp +++ /dev/null @@ -1,48 +0,0 @@ -// Standalone RTPS publisher: announces a writer and periodically publishes std_msgs/msg/UInt32 -// samples. Pair it with rtps_subscriber (C++), python/rtps_subscriber.py, or python/rtps_host.py. -// -// Usage: rtps_publisher [topic] [advertised_ipv4] [period_ms] - -#include -#include - -#include "espp.hpp" -#include "rtps_common.hpp" - -using namespace std::chrono_literals; - -int main(int argc, char **argv) { - espp::Logger logger({.tag = "rtps_publisher", .level = espp::Logger::Verbosity::INFO}); - - const std::string topic = argc > 1 ? argv[1] : "espp/test/counter"; - const std::string address = argc > 2 ? argv[2] : rtps_test::guess_local_ipv4(); - const int period_ms = argc > 3 ? std::atoi(argv[3]) : 1000; - - espp::RtpsParticipant participant({ - .node_name = "espp_publisher", - .participant_id = 10, - .advertised_address = address, - .announce_period = 500ms, - .on_endpoint_discovered = - [&logger](const auto &endpoint) { - logger.info("discovered {} '{}'", endpoint.is_reader ? "reader" : "writer", - endpoint.topic_name); - }, - }); - participant.add_writer({.topic_name = topic}); - - if (!participant.start()) { - logger.error("Failed to start participant (is multicast networking available?)"); - return 1; - } - logger.info("publishing on '{}' from {} every {}ms (Ctrl-C to stop)", topic, address, period_ms); - - uint32_t value = 0; - while (true) { - ++value; - bool sent = participant.publish(topic, rtps_test::serialize_uint32(value)); - logger.info("publish {} -> {}", value, sent ? "sent" : "no destinations yet"); - std::this_thread::sleep_for(std::chrono::milliseconds(period_ms)); - } - return 0; -} diff --git a/pc/tests/rtps_pubsub.cpp b/pc/tests/rtps_pubsub.cpp deleted file mode 100644 index c1670246d9..0000000000 --- a/pc/tests/rtps_pubsub.cpp +++ /dev/null @@ -1,93 +0,0 @@ -// Self-contained RTPS test: two participants (a publisher and a subscriber) in one process exchange -// std_msgs/msg/UInt32 samples over best-effort CDR-over-RTPS, exercising SPDP/SEDP discovery and -// the user-data path end to end. Exits 0 if the subscriber received samples, 1 otherwise. -// -// Usage: rtps_pubsub [advertised_ipv4] [run_seconds] - -#include -#include -#include - -#include "espp.hpp" -#include "rtps_common.hpp" - -using namespace std::chrono_literals; - -int main(int argc, char **argv) { - espp::Logger logger({.tag = "rtps_pubsub", .level = espp::Logger::Verbosity::INFO}); - - const std::string address = argc > 1 ? argv[1] : rtps_test::guess_local_ipv4(); - const int run_seconds = argc > 2 ? std::atoi(argv[2]) : 8; - const std::string topic = "espp/test/counter"; - logger.info("advertising on {} for {}s, topic '{}'", address, run_seconds, topic); - - std::atomic received_count{0}; - std::atomic last_received{0}; - - // --- Subscriber participant --- - espp::RtpsParticipant subscriber({ - .node_name = "espp_pubsub_subscriber", - .participant_id = 11, - .advertised_address = address, - .announce_period = 200ms, - .log_level = espp::Logger::Verbosity::WARN, - }); - subscriber.add_reader({ - .topic_name = topic, - .on_sample = - [&](std::span cdr) { - if (auto value = rtps_test::deserialize_uint32(cdr)) { - received_count++; - last_received = *value; - } - }, - }); - - // --- Publisher participant --- - espp::RtpsParticipant publisher({ - .node_name = "espp_pubsub_publisher", - .participant_id = 10, - .advertised_address = address, - .announce_period = 200ms, - .log_level = espp::Logger::Verbosity::WARN, - }); - publisher.add_writer({.topic_name = topic}); - - if (!subscriber.start() || !publisher.start()) { - logger.error("Failed to start participants (is multicast networking available?)"); - return 1; - } - - // Give SPDP/SEDP discovery a moment to match the writer and reader. - logger.info("waiting for discovery..."); - for (int i = 0; i < 50; i++) { - if (!publisher.discovered_readers().empty() && !subscriber.discovered_writers().empty()) { - break; - } - std::this_thread::sleep_for(100ms); - } - logger.info("discovered {} remote reader(s), {} remote writer(s)", - publisher.discovered_readers().size(), subscriber.discovered_writers().size()); - - uint32_t sent_count = 0; - const auto deadline = std::chrono::steady_clock::now() + std::chrono::seconds(run_seconds); - while (std::chrono::steady_clock::now() < deadline) { - uint32_t value = ++sent_count; - if (publisher.publish(topic, rtps_test::serialize_uint32(value))) { - logger.info("published {} -> received so far {} (last={})", value, received_count.load(), - last_received.load()); - } - std::this_thread::sleep_for(500ms); - } - - publisher.stop(); - subscriber.stop(); - - logger.info("done: sent {}, received {}, last value {}", sent_count, received_count.load(), - last_received.load()); - if (received_count == 0) { - logger.error("subscriber received no samples"); - return 1; - } - return 0; -} diff --git a/pc/tests/rtps_subscriber.cpp b/pc/tests/rtps_subscriber.cpp deleted file mode 100644 index 44c91c7648..0000000000 --- a/pc/tests/rtps_subscriber.cpp +++ /dev/null @@ -1,57 +0,0 @@ -// Standalone RTPS subscriber: announces a reader and prints received std_msgs/msg/UInt32 samples. -// Pair it with rtps_publisher (C++), python/rtps_publisher.py, or python/rtps_host.py. -// -// Usage: rtps_subscriber [topic] [advertised_ipv4] - -#include -#include -#include - -#include "espp.hpp" -#include "rtps_common.hpp" - -using namespace std::chrono_literals; - -int main(int argc, char **argv) { - espp::Logger logger({.tag = "rtps_subscriber", .level = espp::Logger::Verbosity::INFO}); - - const std::string topic = argc > 1 ? argv[1] : "espp/test/counter"; - const std::string address = argc > 2 ? argv[2] : rtps_test::guess_local_ipv4(); - - std::atomic count{0}; - - espp::RtpsParticipant participant({ - .node_name = "espp_subscriber", - .participant_id = 12, - .bind_address = address, - .advertised_address = address, - .announce_period = 500ms, - .on_participant_discovered = - [&logger](const auto &proxy) { - logger.info("discovered participant '{}' at {}", proxy.name, proxy.address); - }, - .log_level = espp::Logger::Verbosity::DEBUG, - }); - participant.add_reader({ - .topic_name = topic, - .on_sample = - [&](std::span cdr) { - if (auto value = rtps_test::deserialize_uint32(cdr)) { - logger.info("received {} (#{})", *value, ++count); - } - }, - }); - - if (!participant.start()) { - logger.error("Failed to start participant (is multicast networking available?)"); - return 1; - } - logger.info("subscribed to '{}' on {} (Ctrl-C to stop)", topic, address); - - while (true) { - std::this_thread::sleep_for(5s); - logger.info("status: {} samples received, {} known publisher(s)", count.load(), - participant.discovered_writers().size()); - } - return 0; -} diff --git a/python/rtps_host.py b/python/rtps_host.py deleted file mode 100644 index 533b5d3c8c..0000000000 --- a/python/rtps_host.py +++ /dev/null @@ -1,1202 +0,0 @@ -#!/usr/bin/env python3 -"""Simple host-side RTPS test harness for the ESPP RTPS component. - -This script speaks the ESPP RTPS discovery wire format plus the standard -CDR-over-RTPS user-data path used by ``RtpsParticipant``. It is useful for: - -1. discovering an embedded ESPP RTPS participant from a PC/host, -2. inspecting SPDP/SEDP announcements, and -3. sending or receiving ``std_msgs/msg/UInt32``-style test samples. User samples - are standard RTPS ``DATA`` submessages whose serializedPayload is the raw - CDR-encapsulated sample; the topic is identified by the writer GUID resolved - through SEDP discovery (no ESPP-specific payload framing). - -Run ``python rtps_host.py --self-test`` to validate the wire-format encoders and -decoders against the firmware's expectations without any network I/O. - -It uses only the Python standard library, so it does not require Python -bindings or a rebuilt host ``lib/`` tree. -""" - -from __future__ import annotations - -import argparse -import hashlib -import ipaddress -import select -import socket -import struct -import sys -import time -from dataclasses import dataclass -from typing import Dict, Iterable, List, Optional, Set, Tuple - - -RTPS_MAGIC = b"RTPS" -PL_CDR_LE = b"\x00\x03\x00\x00" - -PORT_BASE = 7400 -DOMAIN_GAIN = 250 -PARTICIPANT_GAIN = 2 -METATRAFFIC_MULTICAST_OFFSET = 0 -METATRAFFIC_UNICAST_OFFSET = 10 -USER_MULTICAST_OFFSET = 1 -USER_UNICAST_OFFSET = 11 - -DATA_SUBMESSAGE_KIND = 0x15 -DATA_SUBMESSAGE_FLAGS = 0x01 | 0x04 -DATA_SUBMESSAGE_OCTETS_TO_INLINE_QOS = 16 - -RTPS_QOS_RELIABILITY_BEST_EFFORT = 1 -RTPS_QOS_RELIABILITY_RELIABLE = 2 - -KIND_UDP_V4 = 1 -VENDOR_ID = b"\xca\xfe" - -ENTITY_ID_UNKNOWN = b"\x00\x00\x00\x00" -PARTICIPANT_ENTITY_ID = b"\x00\x00\x01\xc1" -SPDP_WRITER_ENTITY_ID = b"\x00\x01\x00\xc2" -SPDP_READER_ENTITY_ID = b"\x00\x01\x00\xc7" -SEDP_PUBLICATIONS_WRITER_ENTITY_ID = b"\x00\x00\x03\xc2" -SEDP_PUBLICATIONS_READER_ENTITY_ID = b"\x00\x00\x03\xc7" -SEDP_SUBSCRIPTIONS_WRITER_ENTITY_ID = b"\x00\x00\x04\xc2" -SEDP_SUBSCRIPTIONS_READER_ENTITY_ID = b"\x00\x00\x04\xc7" -USER_WRITER_NO_KEY_KIND = 0x03 -USER_READER_NO_KEY_KIND = 0x04 - -BUILTIN_ENDPOINT_SET = ( - (1 << 0) - | (1 << 1) - | (1 << 2) - | (1 << 3) - | (1 << 4) - | (1 << 5) - | (1 << 10) - | (1 << 11) -) - -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_RELIABILITY = 0x001A -PID_LIVELINESS = 0x001B -PID_DURABILITY = 0x001D -PID_USER_DATA = 0x002C -PID_MULTICAST_LOCATOR = 0x0030 -PID_UNICAST_LOCATOR = 0x002F -PID_DEFAULT_UNICAST_LOCATOR = 0x0031 -PID_METATRAFFIC_UNICAST_LOCATOR = 0x0032 -PID_METATRAFFIC_MULTICAST_LOCATOR = 0x0033 -PID_HISTORY = 0x0040 -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 - -DEFAULT_LEASE_DURATION_SECONDS = 20 -DEFAULT_LEASE_DURATION_NANOSECONDS = 0 -DEFAULT_MAX_BLOCKING_SECONDS = 0 -DEFAULT_MAX_BLOCKING_NANOSECONDS = 100_000_000 -DEFAULT_TOPIC_PREFIX = "espp/rtps_example" -DEFAULT_REQUEST_TOPIC = f"{DEFAULT_TOPIC_PREFIX}/request" -DEFAULT_RESPONSE_TOPIC = f"{DEFAULT_TOPIC_PREFIX}/response" - - -@dataclass -class PortMapping: - metatraffic_multicast: int - metatraffic_unicast: int - user_multicast: int - user_unicast: int - - -@dataclass -class ParticipantProxy: - participant_guid: bytes - guid_prefix: bytes - name: str - enclave: str - address: str - ports: PortMapping - builtin_endpoints: int - - -@dataclass -class EndpointProxy: - guid: bytes - participant_guid: bytes - topic_name: str - type_name: str - reliability: str - is_reader: bool - expects_inline_qos: bool - unicast_address: str - unicast_port: int - multicast_locators: List[Tuple[str, int]] - - -@dataclass -class WriterConfig: - topic_name: str - type_name: str - reliable: bool - entity_index: int - - -@dataclass -class ReaderConfig: - topic_name: str - type_name: str - reliable: bool - entity_index: int - - -def log(message: str) -> None: - print(message, flush=True) - - -def hex_string(value: bytes) -> str: - return value.hex() - - -def guid_to_string(guid: bytes) -> str: - return hex_string(guid[:12]) + ":" + hex_string(guid[12:]) - - -def entity_id_for_index(entity_index: int, kind: int) -> bytes: - return bytes((0x00, 0x00, 0x10 + entity_index, kind)) - - -def reliability_to_name(reliable: bool) -> str: - return "reliable" if reliable else "best-effort" - - -def ntp_fraction_from_nanoseconds(nanoseconds: int) -> int: - # RTPS Duration_t/Time_t use NTP fraction units of 1/2^32 s, not nanoseconds. - return (nanoseconds << 32) // 1_000_000_000 - - -def padded_parameter_length(length: int) -> int: - # RTPS PL_CDR requires each parameterLength to be a multiple of 4. - return (length + 3) & ~3 - - -def compute_port_mapping(domain_id: int, participant_id: int) -> PortMapping: - base = PORT_BASE + DOMAIN_GAIN * domain_id - participant_offset = PARTICIPANT_GAIN * participant_id - return PortMapping( - metatraffic_multicast=base + METATRAFFIC_MULTICAST_OFFSET, - metatraffic_unicast=base + METATRAFFIC_UNICAST_OFFSET + participant_offset, - user_multicast=base + USER_MULTICAST_OFFSET, - user_unicast=base + USER_UNICAST_OFFSET + participant_offset, - ) - - -def guess_local_ipv4() -> str: - probe = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) - try: - probe.connect(("8.8.8.8", 80)) - return probe.getsockname()[0] - except OSError: - return "127.0.0.1" - finally: - probe.close() - - -def make_guid_prefix(node_name: str, domain_id: int, participant_id: int) -> bytes: - digest = hashlib.sha256(node_name.encode("utf-8")).digest() - return bytes( - ( - participant_id & 0xFF, - (participant_id >> 8) & 0xFF, - domain_id & 0xFF, - (domain_id >> 8) & 0xFF, - ) - ) + digest[:8] - - -def make_guid(prefix: bytes, entity_id: bytes) -> bytes: - return prefix + entity_id - - -def align4(buffer: bytearray) -> None: - while len(buffer) % 4 != 0: - buffer.append(0) - - -def append_parameter_header(buffer: bytearray, pid: int, length: int) -> None: - buffer.extend(struct.pack(" None: - append_parameter_header(buffer, pid, 16) - buffer.extend(guid) - - -def append_parameter_protocol_version(buffer: bytearray) -> None: - append_parameter_header(buffer, PID_PROTOCOL_VERSION, 4) - buffer.extend((2, 3, 0, 0)) - - -def append_parameter_vendor_id(buffer: bytearray) -> None: - append_parameter_header(buffer, PID_VENDORID, 4) - buffer.extend(VENDOR_ID) - buffer.extend((0, 0)) - - -def append_parameter_u32(buffer: bytearray, pid: int, value: int) -> None: - append_parameter_header(buffer, pid, 4) - buffer.extend(struct.pack(" None: - append_parameter_header(buffer, pid, 4) - buffer.extend((1 if value else 0, 0, 0, 0)) - - -def append_parameter_duration(buffer: bytearray, pid: int, seconds: int, nanoseconds: int) -> None: - append_parameter_header(buffer, pid, 8) - buffer.extend(struct.pack(" bytes: - # Locator_t.kind/.port are little-endian in PL_CDR_LE; only the 16-byte address is raw bytes. - locator = bytearray(24) - struct.pack_into(" None: - append_parameter_header(buffer, pid, 24) - buffer.extend(locator_bytes(ip_address, port)) - - -def append_parameter_string_cdr(buffer: bytearray, pid: int, text: str) -> None: - encoded = text.encode("utf-8") - # parameterLength must be a multiple of 4 and includes the trailing CDR padding. - append_parameter_header(buffer, pid, padded_parameter_length(4 + len(encoded) + 1)) - buffer.extend(struct.pack(" None: - append_parameter_header(buffer, pid, padded_parameter_length(4 + len(payload))) - buffer.extend(struct.pack(" None: - append_parameter_header(buffer, PID_RELIABILITY, 12) - kind = RTPS_QOS_RELIABILITY_RELIABLE if reliable else RTPS_QOS_RELIABILITY_BEST_EFFORT - buffer.extend(struct.pack(" None: - append_parameter_header(buffer, PID_DURABILITY, 4) - buffer.extend(struct.pack(" None: - append_parameter_header(buffer, PID_LIVELINESS, 12) - buffer.extend(struct.pack(" None: - append_parameter_header(buffer, PID_HISTORY, 8) - buffer.extend(struct.pack(" None: - append_parameter_header(buffer, PID_KEY_HASH, 16) - buffer.extend(guid) - - -def append_parameter_sentinel(buffer: bytearray) -> None: - append_parameter_header(buffer, PID_SENTINEL, 0) - - -def build_parameter_list_payload(parameter_buffer: bytearray) -> bytes: - return PL_CDR_LE + bytes(parameter_buffer) - - -def build_data_submessage(reader_id: bytes, writer_id: bytes, sequence_number: int, payload: bytes) -> bytes: - high = sequence_number >> 32 - low = sequence_number & 0xFFFFFFFF - submessage_payload = bytearray() - submessage_payload.extend(struct.pack(" bytes: - header = RTPS_MAGIC + bytes((2, 3)) + VENDOR_ID + guid_prefix - return header + build_data_submessage(reader_id, writer_id, sequence_number, payload) - - -def parse_parameter_list(payload: bytes) -> List[tuple[int, bytes]]: - if len(payload) < 4 or payload[:4] != PL_CDR_LE: - return [] - parameters: List[tuple[int, bytes]] = [] - offset = 4 - while offset + 4 <= len(payload): - pid, length = struct.unpack_from(" len(payload): - return [] - value = payload[offset : offset + length] - parameters.append((pid, value)) - offset += length - offset += (4 - (length % 4)) & 0x3 - return parameters - - -def find_parameter(parameters: Iterable[tuple[int, bytes]], pid: int) -> Optional[bytes]: - for candidate_pid, candidate_value in parameters: - if candidate_pid == pid: - return candidate_value - return None - - -def find_parameters(parameters: Iterable[tuple[int, bytes]], pid: int) -> List[bytes]: - return [candidate_value for candidate_pid, candidate_value in parameters if candidate_pid == pid] - - -def parse_guid(value: Optional[bytes]) -> Optional[bytes]: - if value is None or len(value) != 16: - return None - return value - - -def parse_u32_le(value: Optional[bytes]) -> Optional[int]: - if value is None or len(value) < 4: - return None - return struct.unpack_from(" Optional[bool]: - if value is None or not value: - return None - return value[0] != 0 - - -def parse_cdr_string(value: Optional[bytes]) -> Optional[str]: - if value is None or len(value) < 4: - return None - length = struct.unpack_from(" len(value): - return None - raw = value[4 : 4 + length] - if raw.endswith(b"\x00"): - raw = raw[:-1] - return raw.decode("utf-8", errors="replace") - - -def parse_octet_sequence(value: Optional[bytes]) -> Optional[bytes]: - if value is None or len(value) < 4: - return None - length = struct.unpack_from(" len(value): - return None - return value[4 : 4 + length] - - -def parse_locator(value: Optional[bytes]) -> tuple[str, int]: - if value is None or len(value) != 24: - return ("0.0.0.0", 0) - kind = struct.unpack_from(" str: - kind = parse_u32_le(value) - return "reliable" if kind == RTPS_QOS_RELIABILITY_RELIABLE else "best-effort" - - -def extract_enclave(value: Optional[bytes]) -> str: - if not value: - return "/" - text = value.decode("utf-8", errors="replace") - marker = "enclave=" - start = text.find(marker) - if start < 0: - return "/" - start += len(marker) - end = text.find(";", start) - if end < 0: - end = len(text) - return text[start:end] or "/" - - -def serialize_uint32_cdr(value: int) -> bytes: - return b"\x00\x01\x00\x00" + struct.pack(" Optional[int]: - if len(payload) < 8 or payload[:2] != b"\x00\x01": - return None - return struct.unpack_from(" None: - self.args = args - self.ports = compute_port_mapping(args.domain_id, args.participant_id) - self.guid_prefix = make_guid_prefix(args.node_name, args.domain_id, args.participant_id) - self.participant_guid = make_guid(self.guid_prefix, PARTICIPANT_ENTITY_ID) - self.sequence_numbers: Dict[bytes, int] = {} - self.discovered_participants: Dict[bytes, ParticipantProxy] = {} - self.discovered_writers: Dict[bytes, EndpointProxy] = {} - self.discovered_readers: Dict[bytes, EndpointProxy] = {} - self.joined_user_multicast_groups: Set[str] = set() - - self.local_writers = [ - WriterConfig( - topic_name=args.publish_topic, - type_name=args.type_name, - reliable=args.reliable, - entity_index=0, - ) - ] if args.publish_topic else [] - self.local_readers = [ - ReaderConfig( - topic_name=topic_name, - type_name=args.type_name, - reliable=False, - entity_index=index, - ) - for index, topic_name in enumerate(args.subscribe_topic) - ] - - self.metatraffic_multicast_sock = self._create_metatraffic_multicast_socket() - self.metatraffic_unicast_sock = self._create_bound_udp_socket(self.ports.metatraffic_unicast) - self.user_unicast_sock = self._create_bound_udp_socket(self.ports.user_unicast) - self.user_multicast_sock = self._create_user_multicast_socket() - self._configure_multicast_sender(self.metatraffic_unicast_sock) - self._configure_multicast_sender(self.user_unicast_sock) - - self.next_discovery_send = 0.0 - self.next_publish_send = 0.0 - self.last_no_participant_log = 0.0 - self.last_unknown_writer_log = 0.0 - - def _create_bound_udp_socket(self, port: int) -> socket.socket: - sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM, socket.IPPROTO_UDP) - sock.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1) - if hasattr(socket, "SO_REUSEPORT"): - try: - sock.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEPORT, 1) - except OSError: - # Some platforms expose SO_REUSEPORT but reject setting it; this is - # a best-effort optimization and is not required for correctness. - pass - sock.bind((self.args.bind_address, port)) - sock.setblocking(False) - return sock - - def _create_metatraffic_multicast_socket(self) -> socket.socket: - sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM, socket.IPPROTO_UDP) - sock.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1) - if hasattr(socket, "SO_REUSEPORT"): - try: - sock.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEPORT, 1) - except OSError: - # Some platforms expose SO_REUSEPORT but reject setting it; this is - # a best-effort optimization and is not required for correctness. - pass - try: - sock.bind((self.args.multicast_group, self.ports.metatraffic_multicast)) - except OSError: - # Not all platforms allow binding directly to the multicast group - # address, so fall back to the selected local interface address. - sock.bind((self.args.bind_address, self.ports.metatraffic_multicast)) - interface_ip = self.args.multicast_interface or self.args.advertised_address - membership = socket.inet_aton(self.args.multicast_group) + socket.inet_aton(interface_ip) - sock.setsockopt(socket.IPPROTO_IP, socket.IP_ADD_MEMBERSHIP, membership) - sock.setblocking(False) - return sock - - def _create_user_multicast_socket(self) -> socket.socket: - sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM, socket.IPPROTO_UDP) - sock.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1) - if hasattr(socket, "SO_REUSEPORT"): - try: - sock.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEPORT, 1) - except OSError: - # Some platforms expose SO_REUSEPORT but reject setting it; this is - # a best-effort optimization and is not required for correctness. - pass - # Bind to INADDR_ANY, not a unicast address: multicast datagrams are addressed to the group - # (e.g. 239.255.0.11), so a socket bound to a specific unicast interface address will not - # receive them on Linux (and unreliably on macOS). Delivery is decided by the joined - # group(s) + port. This socket may join several user-data groups, so binding to a single - # group address is not an option. - sock.bind(("", self.ports.user_multicast)) - sock.setblocking(False) - return sock - - def _join_user_multicast_group(self, group: str) -> None: - if group in self.joined_user_multicast_groups: - return - interface_ip = self.args.multicast_interface or self.args.advertised_address - membership = socket.inet_aton(group) + socket.inet_aton(interface_ip) - self.user_multicast_sock.setsockopt(socket.IPPROTO_IP, socket.IP_ADD_MEMBERSHIP, membership) - self.joined_user_multicast_groups.add(group) - log(f"[multicast] joined user-data group {group}:{self.ports.user_multicast}") - - def _configure_multicast_sender(self, sock: socket.socket) -> None: - sock.setsockopt(socket.IPPROTO_IP, socket.IP_MULTICAST_TTL, 1) - sock.setsockopt(socket.IPPROTO_IP, socket.IP_MULTICAST_LOOP, 1) - interface_ip = self.args.multicast_interface or self.args.advertised_address - sock.setsockopt(socket.IPPROTO_IP, socket.IP_MULTICAST_IF, socket.inet_aton(interface_ip)) - - def _next_sequence(self, writer_entity_id: bytes) -> int: - value = self.sequence_numbers.get(writer_entity_id, 1) - self.sequence_numbers[writer_entity_id] = value + 1 - return value - - def _local_writer_guid(self, entity_index: int) -> bytes: - return make_guid(self.guid_prefix, entity_id_for_index(entity_index, USER_WRITER_NO_KEY_KIND)) - - def _local_reader_guid(self, entity_index: int) -> bytes: - return make_guid(self.guid_prefix, entity_id_for_index(entity_index, USER_READER_NO_KEY_KIND)) - - def build_spdp_announce_message(self) -> bytes: - parameters = bytearray() - append_parameter_protocol_version(parameters) - append_parameter_vendor_id(parameters) - append_parameter_u32(parameters, PID_DOMAIN_ID, self.args.domain_id) - append_parameter_guid(parameters, PID_PARTICIPANT_GUID, self.participant_guid) - append_parameter_locator( - parameters, - PID_METATRAFFIC_MULTICAST_LOCATOR, - self.args.multicast_group, - self.ports.metatraffic_multicast, - ) - append_parameter_locator( - parameters, - PID_METATRAFFIC_UNICAST_LOCATOR, - self.args.advertised_address, - self.ports.metatraffic_unicast, - ) - append_parameter_locator( - parameters, - PID_DEFAULT_UNICAST_LOCATOR, - self.args.advertised_address, - self.ports.user_unicast, - ) - append_parameter_locator( - parameters, - PID_DEFAULT_MULTICAST_LOCATOR, - self.args.multicast_group, - self.ports.user_multicast, - ) - append_parameter_duration( - parameters, - PID_PARTICIPANT_LEASE_DURATION, - DEFAULT_LEASE_DURATION_SECONDS, - DEFAULT_LEASE_DURATION_NANOSECONDS, - ) - append_parameter_u32(parameters, PID_BUILTIN_ENDPOINT_SET, BUILTIN_ENDPOINT_SET) - append_parameter_octet_sequence( - parameters, - PID_USER_DATA, - f"enclave={self.args.enclave};".encode("utf-8"), - ) - append_parameter_string_cdr(parameters, PID_ENTITY_NAME, self.args.node_name) - append_parameter_sentinel(parameters) - payload = build_parameter_list_payload(parameters) - return build_rtps_message( - self.guid_prefix, - ENTITY_ID_UNKNOWN, - SPDP_WRITER_ENTITY_ID, - self._next_sequence(SPDP_WRITER_ENTITY_ID), - payload, - ) - - def build_sedp_publication_message(self, writer: WriterConfig) -> bytes: - guid = self._local_writer_guid(writer.entity_index) - parameters = bytearray() - append_parameter_guid(parameters, PID_ENDPOINT_GUID, guid) - append_parameter_locator(parameters, PID_UNICAST_LOCATOR, self.args.advertised_address, self.ports.user_unicast) - append_parameter_guid(parameters, PID_PARTICIPANT_GUID, self.participant_guid) - append_parameter_string_cdr(parameters, PID_TOPIC_NAME, writer.topic_name) - append_parameter_string_cdr(parameters, PID_TYPE_NAME, writer.type_name) - append_parameter_key_hash(parameters, guid) - append_parameter_u32(parameters, PID_TYPE_MAX_SIZE_SERIALIZED, 8) - append_parameter_protocol_version(parameters) - append_parameter_vendor_id(parameters) - append_parameter_durability(parameters) - append_parameter_liveliness(parameters) - append_parameter_reliability(parameters, writer.reliable) - append_parameter_history(parameters) - append_parameter_sentinel(parameters) - payload = build_parameter_list_payload(parameters) - return build_rtps_message( - self.guid_prefix, - SEDP_PUBLICATIONS_READER_ENTITY_ID, - SEDP_PUBLICATIONS_WRITER_ENTITY_ID, - self._next_sequence(SEDP_PUBLICATIONS_WRITER_ENTITY_ID), - payload, - ) - - def build_sedp_subscription_message(self, reader: ReaderConfig) -> bytes: - guid = self._local_reader_guid(reader.entity_index) - parameters = bytearray() - append_parameter_guid(parameters, PID_ENDPOINT_GUID, guid) - append_parameter_locator(parameters, PID_UNICAST_LOCATOR, self.args.advertised_address, self.ports.user_unicast) - append_parameter_bool(parameters, PID_EXPECTS_INLINE_QOS, False) - append_parameter_guid(parameters, PID_PARTICIPANT_GUID, self.participant_guid) - append_parameter_string_cdr(parameters, PID_TOPIC_NAME, reader.topic_name) - append_parameter_string_cdr(parameters, PID_TYPE_NAME, reader.type_name) - append_parameter_key_hash(parameters, guid) - append_parameter_protocol_version(parameters) - append_parameter_vendor_id(parameters) - append_parameter_durability(parameters) - append_parameter_liveliness(parameters) - append_parameter_reliability(parameters, reader.reliable) - append_parameter_history(parameters) - append_parameter_sentinel(parameters) - payload = build_parameter_list_payload(parameters) - return build_rtps_message( - self.guid_prefix, - SEDP_SUBSCRIPTIONS_READER_ENTITY_ID, - SEDP_SUBSCRIPTIONS_WRITER_ENTITY_ID, - self._next_sequence(SEDP_SUBSCRIPTIONS_WRITER_ENTITY_ID), - payload, - ) - - def build_data_message(self, writer: WriterConfig, cdr_payload: bytes) -> bytes: - # Standard RTPS: the DATA serializedPayload is exactly the CDR-encapsulated sample. - writer_entity_id = entity_id_for_index(writer.entity_index, USER_WRITER_NO_KEY_KIND) - return build_rtps_message( - self.guid_prefix, - ENTITY_ID_UNKNOWN, - writer_entity_id, - self._next_sequence(writer_entity_id), - cdr_payload, - ) - - def send_spdp_announce_now(self) -> None: - payload = self.build_spdp_announce_message() - self.metatraffic_unicast_sock.sendto( - payload, - (self.args.multicast_group, self.ports.metatraffic_multicast), - ) - - def send_sedp_announcements_to(self, participant: ParticipantProxy) -> None: - target = (participant.address, participant.ports.metatraffic_unicast) - if participant.ports.metatraffic_unicast == 0 or not participant.address: - return - for writer in self.local_writers: - self.metatraffic_unicast_sock.sendto(self.build_sedp_publication_message(writer), target) - for reader in self.local_readers: - self.metatraffic_unicast_sock.sendto(self.build_sedp_subscription_message(reader), target) - - def send_discovery_now(self) -> None: - self.send_spdp_announce_now() - for participant in list(self.discovered_participants.values()): - self.send_sedp_announcements_to(participant) - - def publish_now(self) -> None: - if not self.local_writers: - return - if not self._publish_value(self.local_writers[0], self.args.publish_value): - now = time.monotonic() - if now - self.last_no_participant_log > 2.0: - log( - f"[publish] no discovered participants yet for topic '{self.local_writers[0].topic_name}', " - "waiting for SPDP" - ) - self.last_no_participant_log = now - else: - log( - f"[publish] sent {self.args.publish_value} on '{self.local_writers[0].topic_name}' " - f"using {len(self._build_user_targets(self.local_writers[0]))} discovered target(s)" - ) - - def _build_user_targets(self, writer: WriterConfig) -> List[Tuple[str, int]]: - targets: List[Tuple[str, int]] = [] - for reader in self.discovered_readers.values(): - if reader.topic_name != writer.topic_name: - continue - if reader.multicast_locators: - for multicast_address, multicast_port in reader.multicast_locators: - target = (multicast_address, multicast_port) - if target not in targets: - targets.append(target) - continue - if reader.unicast_port > 0 and reader.unicast_address: - target = (reader.unicast_address, reader.unicast_port) - if target not in targets: - targets.append(target) - if targets: - return targets - for participant in self.discovered_participants.values(): - target = (participant.address, participant.ports.user_unicast) - if participant.address and participant.ports.user_unicast > 0 and target not in targets: - targets.append(target) - return targets - - def _publish_value(self, writer: WriterConfig, value: int, target: Optional[tuple[str, int]] = None) -> bool: - payload = self.build_data_message(writer, serialize_uint32_cdr(value)) - if target is not None: - self.user_unicast_sock.sendto(payload, target) - return True - targets = self._build_user_targets(writer) - if not targets: - return False - for destination in targets: - self.user_unicast_sock.sendto(payload, destination) - return True - - def handle_metatraffic_packet(self, packet: bytes, sender_ip: str) -> None: - for _guid_prefix, writer_id, serialized_payload in parse_rtps_data_messages(packet): - parameters = parse_parameter_list(serialized_payload) - if not parameters: - continue - - if writer_id == SPDP_WRITER_ENTITY_ID: - self._handle_spdp(parameters, sender_ip) - elif writer_id == SEDP_PUBLICATIONS_WRITER_ENTITY_ID: - self._handle_sedp(parameters, sender_ip, is_reader=False) - elif writer_id == SEDP_SUBSCRIPTIONS_WRITER_ENTITY_ID: - self._handle_sedp(parameters, sender_ip, is_reader=True) - - def _handle_spdp(self, parameters: List[tuple[int, bytes]], sender_ip: str) -> None: - participant_guid = parse_guid(find_parameter(parameters, PID_PARTICIPANT_GUID)) - if participant_guid is None or participant_guid[:12] == self.guid_prefix: - return - - meta_ip, meta_uc_port = parse_locator(find_parameter(parameters, PID_METATRAFFIC_UNICAST_LOCATOR)) - _, meta_mc_port = parse_locator(find_parameter(parameters, PID_METATRAFFIC_MULTICAST_LOCATOR)) - user_ip, user_uc_port = parse_locator(find_parameter(parameters, PID_DEFAULT_UNICAST_LOCATOR)) - _, user_mc_port = parse_locator(find_parameter(parameters, PID_DEFAULT_MULTICAST_LOCATOR)) - - participant = ParticipantProxy( - participant_guid=participant_guid, - guid_prefix=participant_guid[:12], - name=parse_cdr_string(find_parameter(parameters, PID_ENTITY_NAME)) or "", - enclave=extract_enclave(parse_octet_sequence(find_parameter(parameters, PID_USER_DATA))), - address=user_ip if user_ip != "0.0.0.0" else sender_ip, - ports=PortMapping( - metatraffic_multicast=meta_mc_port, - metatraffic_unicast=meta_uc_port, - user_multicast=user_mc_port, - user_unicast=user_uc_port, - ), - builtin_endpoints=parse_u32_le(find_parameter(parameters, PID_BUILTIN_ENDPOINT_SET)) or 0, - ) - - is_new = participant_guid not in self.discovered_participants - self.discovered_participants[participant_guid] = participant - label = participant.name or hex_string(participant.guid_prefix) - log( - f"[spdp] participant '{label}' at {participant.address} " - f"(meta={participant.ports.metatraffic_unicast}, user={participant.ports.user_unicast}, " - f"enclave={participant.enclave})" - ) - if is_new: - self.send_sedp_announcements_to(participant) - - def _handle_sedp(self, parameters: List[tuple[int, bytes]], sender_ip: str, is_reader: bool) -> None: - endpoint_guid = parse_guid(find_parameter(parameters, PID_ENDPOINT_GUID)) - if endpoint_guid is None or endpoint_guid[:12] == self.guid_prefix: - return - - participant_guid = parse_guid(find_parameter(parameters, PID_PARTICIPANT_GUID)) - if participant_guid is None: - participant_guid = endpoint_guid[:12] + PARTICIPANT_ENTITY_ID - - endpoint_ip, endpoint_port = parse_locator(find_parameter(parameters, PID_UNICAST_LOCATOR)) - multicast_locators = [ - parse_locator(value) - for value in find_parameters(parameters, PID_MULTICAST_LOCATOR) - ] - multicast_locators = [ - (multicast_address, multicast_port) - for multicast_address, multicast_port in multicast_locators - if multicast_address != "0.0.0.0" and multicast_port > 0 - ] - endpoint = EndpointProxy( - guid=endpoint_guid, - participant_guid=participant_guid, - topic_name=parse_cdr_string(find_parameter(parameters, PID_TOPIC_NAME)) or "", - type_name=parse_cdr_string(find_parameter(parameters, PID_TYPE_NAME)) or "", - reliability=parse_reliability(find_parameter(parameters, PID_RELIABILITY)), - is_reader=is_reader, - expects_inline_qos=parse_bool(find_parameter(parameters, PID_EXPECTS_INLINE_QOS)) or False, - unicast_address=endpoint_ip if endpoint_ip != "0.0.0.0" else sender_ip, - unicast_port=endpoint_port, - multicast_locators=multicast_locators, - ) - endpoint_map = self.discovered_readers if is_reader else self.discovered_writers - is_new = endpoint_guid not in endpoint_map - endpoint_map[endpoint_guid] = endpoint - if not is_reader: - subscribed_topics = {reader.topic_name for reader in self.local_readers} - if endpoint.topic_name in subscribed_topics: - for multicast_address, multicast_port in endpoint.multicast_locators: - if multicast_port == self.ports.user_multicast: - self._join_user_multicast_group(multicast_address) - if is_new: - kind = "reader" if is_reader else "writer" - log( - f"[sedp] {kind} topic='{endpoint.topic_name}' type='{endpoint.type_name}' " - f"reliability={endpoint.reliability} participant={guid_to_string(endpoint.participant_guid)}" - ) - - def handle_user_packet(self, packet: bytes, sender_ip: str, sender_port: int) -> None: - subscribed_topics = {reader.topic_name for reader in self.local_readers} - for guid_prefix, writer_id, serialized_payload in parse_rtps_data_messages(packet): - # Standard RTPS: resolve the topic from the writer GUID via SEDP discovery state. - writer_guid = guid_prefix + writer_id - writer = self.discovered_writers.get(writer_guid) - if writer is None: - # Sample arrived before its writer was discovered via SEDP; drop it (best-effort). - # Surface it (rate-limited) so a missing SEDP exchange is visible rather than silent. - now = time.monotonic() - if now - self.last_unknown_writer_log > 2.0: - log( - f"[data] received {len(serialized_payload)}-byte sample from UNDISCOVERED " - f"writer {guid_to_string(writer_guid)} at {sender_ip}:{sender_port}; cannot " - f"route without SEDP (discovered_writers={len(self.discovered_writers)})" - ) - self.last_unknown_writer_log = now - continue - topic_name = writer.topic_name - if topic_name not in subscribed_topics: - continue - maybe_value = deserialize_uint32_cdr(serialized_payload) - if maybe_value is None: - continue - log( - f"[data] topic='{topic_name}' value={maybe_value} reliability={writer.reliability} " - f"from {sender_ip}:{sender_port} writer={hex_string(writer_id)}" - ) - if self.args.echo_received and self.local_writers: - out_writer = self.local_writers[0] - if self._publish_value(out_writer, maybe_value): - log(f"[echo] responded with value={maybe_value} on '{out_writer.topic_name}'") - else: - self._publish_value(out_writer, maybe_value, (sender_ip, sender_port)) - log( - f"[echo] responded with value={maybe_value} on '{out_writer.topic_name}' " - f"to {sender_ip}:{sender_port}" - ) - - def run(self) -> None: - start_time = time.monotonic() - self.next_discovery_send = start_time - self.next_publish_send = start_time + self.args.publish_interval - - log( - "Starting RTPS host harness\n" - f" node: {self.args.node_name}\n" - f" advertised address: {self.args.advertised_address}\n" - f" domain/participant: {self.args.domain_id}/{self.args.participant_id}\n" - f" ports: meta_mc={self.ports.metatraffic_multicast}, meta_uc={self.ports.metatraffic_unicast}, " - f"user_mc={self.ports.user_multicast}, user_uc={self.ports.user_unicast}" - ) - if self.local_readers: - log(" readers: " + ", ".join(reader.topic_name for reader in self.local_readers)) - if self.local_writers: - writer = self.local_writers[0] - writer_mode = "echo responder" if self.args.echo_received else "periodic publisher" - interval_text = ( - f", publish value={self.args.publish_value} every {self.args.publish_interval:.2f}s" - if self.args.publish_interval > 0 - else "" - ) - log(f" writer: {writer.topic_name} ({reliability_to_name(writer.reliable)}, {writer_mode}{interval_text})") - - try: - while True: - now = time.monotonic() - if now >= self.next_discovery_send: - self.send_discovery_now() - self.next_discovery_send = now + self.args.announce_period - if self.local_writers and self.args.publish_interval > 0 and now >= self.next_publish_send: - self.publish_now() - self.next_publish_send = now + self.args.publish_interval - if self.args.duration > 0 and now - start_time >= self.args.duration: - break - - readable, _, _ = select.select( - [ - self.metatraffic_multicast_sock, - self.metatraffic_unicast_sock, - self.user_unicast_sock, - self.user_multicast_sock, - ], - [], - [], - 0.2, - ) - for sock in readable: - packet, sender = sock.recvfrom(4096) - sender_ip, sender_port = sender[0], sender[1] - if sock is self.user_unicast_sock or sock is self.user_multicast_sock: - self.handle_user_packet(packet, sender_ip, sender_port) - else: - self.handle_metatraffic_packet(packet, sender_ip) - except KeyboardInterrupt: - log("Stopping RTPS host harness") - finally: - self.close() - - def close(self) -> None: - for sock in ( - self.metatraffic_multicast_sock, - self.metatraffic_unicast_sock, - self.user_unicast_sock, - self.user_multicast_sock, - ): - try: - sock.close() - except OSError as exc: - log(f"[close] ignoring socket close failure for {sock!r}: {exc}") - - -def parse_rtps_data_messages(packet: bytes) -> List[tuple[bytes, bytes, bytes]]: - """Return (guid_prefix, writer_id, serialized_payload) for each DATA submessage.""" - if len(packet) < 20 or not packet.startswith(RTPS_MAGIC): - return [] - guid_prefix = packet[8:20] - offset = 20 - messages: List[tuple[bytes, bytes, bytes]] = [] - while offset + 4 <= len(packet): - kind = packet[offset] - flags = packet[offset + 1] - length = struct.unpack_from(" len(packet): - break - payload = packet[offset : offset + length] - offset += length - if kind != DATA_SUBMESSAGE_KIND or (flags & 0x04) == 0: - continue - if len(payload) < 20: - continue - extra_flags, octets_to_inline_qos = struct.unpack_from(" int: - """Validate the wire-format encoders/decoders against firmware expectations (no network I/O).""" - failures: List[str] = [] - - def check(name: str, condition: bool) -> None: - log(f" [{'PASS' if condition else 'FAIL'}] {name}") - if not condition: - failures.append(name) - - # Locators: kind/port are little-endian, address is raw network-order bytes; round-trips. - loc = locator_bytes("192.168.1.5", 7411) - check("locator kind is little-endian", loc[:4] == struct.pack(" 16 - append_parameter_string_cdr(params, PID_TYPE_NAME, "std_msgs::msg::dds_::UInt32_") - append_parameter_octet_sequence(params, PID_USER_DATA, b"enclave=/;") # body 4+10=14 -> 16 - offset = 0 - aligned = True - while offset + 4 <= len(params): - pid, length = struct.unpack_from(" argparse.Namespace: - parser = argparse.ArgumentParser( - description=( - "Discover an ESPP RTPS participant from a PC/host and optionally " - "exchange temporary UInt32 user-data samples." - ) - ) - parser.add_argument("--node-name", default="python_rtps_host", help="Local participant name") - parser.add_argument("--domain-id", type=int, default=0, help="RTPS domain id") - parser.add_argument("--participant-id", type=int, default=10, help="Local participant id") - parser.add_argument( - "--bind-address", - default=None, - help="Local bind address (defaults to the advertised address rather than all interfaces)", - ) - parser.add_argument( - "--advertised-address", - default=None, - help="IPv4 address to advertise to peers (defaults to best-effort local IPv4)", - ) - parser.add_argument( - "--multicast-interface", - default=None, - help="IPv4 interface to use for multicast join/send (defaults to advertised address)", - ) - parser.add_argument("--multicast-group", default="239.255.0.1", help="RTPS metatraffic multicast group") - parser.add_argument("--enclave", default="/", help="Enclave string advertised in SPDP user data") - parser.add_argument( - "--subscribe-topic", - action="append", - default=None, - help=f"Topic name to advertise as a local reader (repeatable). Defaults to {DEFAULT_REQUEST_TOPIC}.", - ) - parser.add_argument( - "--publish-topic", - default=None, - help=f"Topic name to publish as a local writer. Defaults to {DEFAULT_RESPONSE_TOPIC}.", - ) - parser.add_argument("--publish-value", type=int, default=42, help="UInt32 value to publish") - parser.add_argument( - "--publish-interval", - type=float, - default=0.0, - help="Seconds between periodic publish attempts when --publish-topic is set (0 disables periodic publishing)", - ) - parser.set_defaults(echo_received=True) - parser.add_argument( - "--echo-received", - dest="echo_received", - action="store_true", - help="Echo received subscribed-topic values back on the publish topic (enabled by default)", - ) - parser.add_argument( - "--no-echo-received", - dest="echo_received", - action="store_false", - help="Disable request/response echo behavior and only use periodic publishing", - ) - parser.add_argument("--reliable", action="store_true", help="Mark the local writer as reliable") - parser.add_argument("--type-name", default="std_msgs/msg/UInt32", help="Advertised type name") - parser.add_argument( - "--announce-period", - type=float, - default=1.0, - help="Seconds between periodic SPDP/SEDP discovery announcements", - ) - parser.add_argument( - "--duration", - type=float, - default=0.0, - help="Stop after this many seconds (0 = run until Ctrl+C)", - ) - args = parser.parse_args() - - if args.subscribe_topic is None: - args.subscribe_topic = [DEFAULT_REQUEST_TOPIC] - if args.publish_topic is None: - args.publish_topic = DEFAULT_RESPONSE_TOPIC - if args.advertised_address is None: - args.advertised_address = guess_local_ipv4() - if args.bind_address is None: - args.bind_address = args.advertised_address - try: - ipaddress.IPv4Address(args.bind_address) - ipaddress.IPv4Address(args.advertised_address) - ipaddress.IPv4Address(args.multicast_interface or args.advertised_address) - ipaddress.IPv4Address(args.multicast_group) - except ipaddress.AddressValueError as exc: - parser.error(str(exc)) - if args.publish_interval < 0: - parser.error("--publish-interval must be >= 0") - if args.announce_period <= 0: - parser.error("--announce-period must be > 0") - return args - - -def main() -> int: - # Handle --self-test before full argument parsing so it needs no network/address configuration. - if "--self-test" in sys.argv: - return run_self_test() - args = parse_args() - harness = RtpsHostHarness(args) - harness.run() - return 0 - - -if __name__ == "__main__": - sys.exit(main()) diff --git a/python/rtps_publisher.py b/python/rtps_publisher.py index 1434cc72af..2ec39ff7a8 100644 --- a/python/rtps_publisher.py +++ b/python/rtps_publisher.py @@ -1,14 +1,13 @@ #!/usr/bin/env python3 -"""Standalone RTPS publisher using the espp Python library. +"""Standalone RTPS publisher using the espp Python library (embeddedRTPS engine). -Announces a writer and periodically publishes std_msgs/msg/UInt32 samples. Pair it with -rtps_subscriber.py, the C++ rtps_subscriber, or rtps_host.py. +Publishes CDR string samples on a topic; defaults follow the ROS 2 conventions for +std_msgs/String on /chatter, so `ros2 topic echo /chatter std_msgs/msg/String` +(with rmw_fastrtps) receives them. Pair it with rtps_subscriber.py or any DDS peer. -Usage: python rtps_publisher.py [topic] [advertised_ipv4] [period_seconds] +Usage: python rtps_publisher.py [topic] [type] [interface_ipv4] [period_seconds] """ -import datetime -import socket import struct import sys import time @@ -16,54 +15,39 @@ import espp -def guess_local_ipv4() -> str: - probe = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) - try: - probe.connect(("8.8.8.8", 53)) - return probe.getsockname()[0] - except OSError: - return "127.0.0.1" - finally: - probe.close() - - -def serialize_uint32(value: int) -> bytes: - # Little-endian CDR (XCDR1) with a 4-byte encapsulation header — the wire format of - # std_msgs/msg/UInt32. Plain struct.pack; no native bindings needed for CDR payloads. - return b"\x00\x01\x00\x00" + struct.pack(" bytes: + # Little-endian classic CDR (XCDR1) with a 4-byte encapsulation header - the wire + # format of std_msgs/msg/String: uint32 length (incl. null) + bytes + null. + data = text.encode() + b"\x00" + return b"\x00\x01\x00\x00" + struct.pack(" int: - R = espp.RtpsParticipant - topic = sys.argv[1] if len(sys.argv) > 1 else "espp/test/counter" - address = sys.argv[2] if len(sys.argv) > 2 else guess_local_ipv4() - period = float(sys.argv[3]) if len(sys.argv) > 3 else 1.0 - - cfg = R.Config() - cfg.node_name = "py_publisher" - cfg.participant_id = 20 - cfg.advertised_address = address - cfg.announce_period = datetime.timedelta(milliseconds=500) - cfg.on_endpoint_discovered = lambda e: print( - f"discovered {'reader' if e.is_reader else 'writer'} '{e.topic_name}'") - participant = R(cfg) - wc = R.WriterConfig() - wc.topic_name = topic - participant.add_writer(wc) + topic = sys.argv[1] if len(sys.argv) > 1 else "rt/chatter" + type_name = sys.argv[2] if len(sys.argv) > 2 else "std_msgs::msg::dds_::String_" + interface = sys.argv[3] if len(sys.argv) > 3 else "" # "" -> auto-detect + period = float(sys.argv[4]) if len(sys.argv) > 4 else 0.5 + R = espp.RtpsParticipant + participant = R(R.Config(interface_address=interface, log_level=espp.Logger.Verbosity.info)) if not participant.start(): - print("Failed to start participant (is multicast networking available?)") + print("failed to start participant") return 1 - print(f"publishing on '{topic}' from {address} every {period}s (Ctrl-C to stop)") + if not participant.add_writer(topic=topic, type_name=type_name, reliable=True): + print("failed to add writer") + return 1 + print(f"publishing '{topic}' ({type_name}) every {period}s; ctrl-c to stop") - value = 0 + count = 0 try: while True: - value += 1 - sent = participant.publish(topic, serialize_uint32(value)) - print(f"publish {value} -> {'sent' if sent else 'no destinations yet'}") + if participant.publish(topic, serialize_string(f"espp python {count}")): + count += 1 + print(f"sent {count}") time.sleep(period) except KeyboardInterrupt: + pass + finally: participant.stop() return 0 diff --git a/python/rtps_pubsub.py b/python/rtps_pubsub.py deleted file mode 100644 index 2ec8c197e6..0000000000 --- a/python/rtps_pubsub.py +++ /dev/null @@ -1,114 +0,0 @@ -#!/usr/bin/env python3 -"""Self-contained RTPS test using the espp Python library. - -Creates two participants (a publisher and a subscriber) in one process and exchanges -std_msgs/msg/UInt32 samples over best-effort CDR-over-RTPS, exercising SPDP/SEDP discovery and the -user-data path end to end. Exits 0 if the subscriber received samples, 1 otherwise. - -Usage: python rtps_pubsub.py [advertised_ipv4] [run_seconds] -""" - -import datetime -import socket -import struct -import sys -import time - -import espp - - -def guess_local_ipv4() -> str: - # RTPS discovery is multicast, so a real interface address (not 127.0.0.1) is needed. - probe = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) - try: - probe.connect(("8.8.8.8", 53)) - return probe.getsockname()[0] - except OSError: - return "127.0.0.1" - finally: - probe.close() - - -def serialize_uint32(value: int) -> bytes: - # Little-endian CDR (XCDR1) with a 4-byte encapsulation header — the wire format of - # std_msgs/msg/UInt32. Plain struct.pack; no native bindings needed for CDR payloads. - return b"\x00\x01\x00\x00" + struct.pack("" - return struct.unpack_from(endian + "I", data, 4)[0] - - -def main() -> int: - R = espp.RtpsParticipant - address = sys.argv[1] if len(sys.argv) > 1 else guess_local_ipv4() - run_seconds = int(sys.argv[2]) if len(sys.argv) > 2 else 8 - topic = "espp/test/counter" - print(f"advertising on {address} for {run_seconds}s, topic '{topic}'") - - stats = {"received": 0, "last": 0} - - # --- Subscriber --- - sub_cfg = R.Config() - sub_cfg.node_name = "py_pubsub_subscriber" - sub_cfg.participant_id = 21 - sub_cfg.advertised_address = address - sub_cfg.announce_period = datetime.timedelta(milliseconds=200) - sub_cfg.log_level = espp.Logger.Verbosity.warn - subscriber = R(sub_cfg) - - def on_sample(cdr: bytes): - value = deserialize_uint32(cdr) - if value is not None: - stats["received"] += 1 - stats["last"] = value - - rc = R.ReaderConfig() - rc.topic_name = topic - rc.on_sample = on_sample - subscriber.add_reader(rc) - - # --- Publisher --- - pub_cfg = R.Config() - pub_cfg.node_name = "py_pubsub_publisher" - pub_cfg.participant_id = 20 - pub_cfg.advertised_address = address - pub_cfg.announce_period = datetime.timedelta(milliseconds=200) - pub_cfg.log_level = espp.Logger.Verbosity.warn - publisher = R(pub_cfg) - wc = R.WriterConfig() - wc.topic_name = topic - publisher.add_writer(wc) - - if not subscriber.start() or not publisher.start(): - print("Failed to start participants (is multicast networking available?)") - return 1 - - print("waiting for discovery...") - for _ in range(50): - if publisher.discovered_readers() and subscriber.discovered_writers(): - break - time.sleep(0.1) - print(f"discovered {len(publisher.discovered_readers())} remote reader(s), " - f"{len(subscriber.discovered_writers())} remote writer(s)") - - sent = 0 - deadline = time.monotonic() + run_seconds - while time.monotonic() < deadline: - sent += 1 - if publisher.publish(topic, serialize_uint32(sent)): - print(f"published {sent} -> received so far {stats['received']} (last={stats['last']})") - time.sleep(0.5) - - publisher.stop() - subscriber.stop() - print(f"done: sent {sent}, received {stats['received']}, last value {stats['last']}") - return 0 if stats["received"] else 1 - - -if __name__ == "__main__": - sys.exit(main()) diff --git a/python/rtps_subscriber.py b/python/rtps_subscriber.py index 99dd384c31..355e41f3ed 100644 --- a/python/rtps_subscriber.py +++ b/python/rtps_subscriber.py @@ -1,14 +1,13 @@ #!/usr/bin/env python3 -"""Standalone RTPS subscriber using the espp Python library. +"""Standalone RTPS subscriber using the espp Python library (embeddedRTPS engine). -Announces a reader and prints received std_msgs/msg/UInt32 samples. Pair it with rtps_publisher.py, -the C++ rtps_publisher, or rtps_host.py. +Subscribes to CDR string samples; defaults follow the ROS 2 conventions for +std_msgs/String on /chatter, so `ros2 topic pub /chatter std_msgs/msg/String ...` +(with rmw_fastrtps) is received. Pair it with rtps_publisher.py or any DDS peer. -Usage: python rtps_subscriber.py [topic] [advertised_ipv4] +Usage: python rtps_subscriber.py [topic] [type] [interface_ipv4] [run_seconds] """ -import datetime -import socket import struct import sys import time @@ -16,65 +15,50 @@ import espp -def guess_local_ipv4() -> str: - probe = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) - try: - probe.connect(("8.8.8.8", 53)) - return probe.getsockname()[0] - except OSError: - return "127.0.0.1" - finally: - probe.close() - - -def deserialize_uint32(data: bytes): - # Accepts either endianness; the encapsulation identifier's low bit selects little-endian. - if len(data) < 8 or data[0] != 0x00 or data[1] not in (0x00, 0x01): - return None # int, or None on failure - endian = "<" if data[1] & 1 else ">" - return struct.unpack_from(endian + "I", data, 4)[0] +def deserialize_string(data: bytes): + # 4-byte encapsulation header + uint32 length + bytes (incl. trailing null). + if len(data) < 8: + return None + little_endian = data[1] & 0x01 + (length,) = struct.unpack("I", data[4:8]) + if length < 1 or 8 + length > len(data): + return None + return data[8 : 8 + length - 1].decode(errors="replace") def main() -> int: - R = espp.RtpsParticipant - topic = sys.argv[1] if len(sys.argv) > 1 else "espp/test/counter" - address = sys.argv[2] if len(sys.argv) > 2 else guess_local_ipv4() + topic = sys.argv[1] if len(sys.argv) > 1 else "rt/chatter" + type_name = sys.argv[2] if len(sys.argv) > 2 else "std_msgs::msg::dds_::String_" + interface = sys.argv[3] if len(sys.argv) > 3 else "" # "" -> auto-detect + run_seconds = float(sys.argv[4]) if len(sys.argv) > 4 else 30.0 - count = {"n": 0} + stats = {"received": 0} - cfg = R.Config() - cfg.node_name = "py_subscriber" - cfg.participant_id = 22 - cfg.advertised_address = address - cfg.announce_period = datetime.timedelta(milliseconds=500) - cfg.on_participant_discovered = lambda p: print( - f"discovered participant '{p.name}' at {p.address}") - participant = R(cfg) - - def on_sample(cdr: bytes): - value = deserialize_uint32(cdr) - if value is not None: - count["n"] += 1 - print(f"received {value} (#{count['n']})") - - rc = R.ReaderConfig() - rc.topic_name = topic - rc.on_sample = on_sample - participant.add_reader(rc) + def on_sample(data: bytes) -> None: + text = deserialize_string(data) + if text is not None: + stats["received"] += 1 + print(f"received {stats['received']}: {text!r}") + R = espp.RtpsParticipant + participant = R(R.Config(interface_address=interface, log_level=espp.Logger.Verbosity.info)) if not participant.start(): - print("Failed to start participant (is multicast networking available?)") + print("failed to start participant") + return 1 + if not participant.add_reader( + topic=topic, type_name=type_name, reliable=True, on_sample=on_sample + ): + print("failed to add reader") return 1 - print(f"subscribed to '{topic}' on {address} (Ctrl-C to stop)") + print(f"subscribed to '{topic}' ({type_name}) for {run_seconds}s") try: - while True: - time.sleep(5) - print(f"status: {count['n']} samples received, " - f"{len(participant.discovered_writers())} known publisher(s)") + time.sleep(run_seconds) except KeyboardInterrupt: - participant.stop() - return 0 + pass + participant.stop() + print(f"done; received={stats['received']}") + return stats["received"] == 0 if __name__ == "__main__":