diff --git a/.gitignore b/.gitignore index fcb575a86..d50af212d 100644 --- a/.gitignore +++ b/.gitignore @@ -56,3 +56,9 @@ managed_components/ # docker interop harness build tree (bind-mounted, built in-container) pc/build-linux/ python/env/ + +# Locally generated documentation output (Doxygen/Sphinx build of doc/ -> docs/). +docs/ + +# Local example-build helper script (not part of the repo). +build_examples.sh diff --git a/components/rtps_embedded/CMakeLists.txt b/components/rtps_embedded/CMakeLists.txt index bc784c654..0465b1aa0 100644 --- a/components/rtps_embedded/CMakeLists.txt +++ b/components/rtps_embedded/CMakeLists.txt @@ -23,3 +23,34 @@ idf_component_register( base_component cdr task thread_pool socket ) +# Select the RTPS static-limits profile from Kconfig (see Kconfig in this +# component). The "embedded" profile is the default and is byte-identical to the +# historical behavior: it defines no RTPS_CONFIG_HEADER, so config.hpp selects +# rtps/config_esp32.hpp via ESP_PLATFORM. The relaxed "host" / "host_large" +# profiles override the profile header. These are capacity-only caps and do not +# change any bytes on the wire. +if(CONFIG_RTPS_LIMITS_PROFILE_HOST) + target_compile_definitions(${COMPONENT_LIB} PUBLIC RTPS_CONFIG_HEADER="rtps/config_desktop.hpp") +elseif(CONFIG_RTPS_LIMITS_PROFILE_HOST_LARGE) + target_compile_definitions(${COMPONENT_LIB} PUBLIC RTPS_CONFIG_HEADER="rtps/config_host_large.hpp") +endif() + +# Storage policy is orthogonal to the limits profile above: dynamic (heap, +# grow-on-full) storage is an explicit ESP opt-in (default static) so a relaxed +# limits profile never silently switches the MCU to heap-backed history. The +# limits headers no longer define RTPS_STORAGE_DYNAMIC themselves; it is set here +# on ESP and defaulted on in config.hpp for host/PC builds. +if(CONFIG_RTPS_STORAGE_DYNAMIC) + target_compile_definitions(${COMPONENT_LIB} PUBLIC RTPS_STORAGE_DYNAMIC) +endif() + +# Best-effort DATA_FRAG fragmentation is opt-in on ESP targets (Kconfig, default +# off) so the MCU pays nothing for it by default. When enabled, define +# RTPS_ENABLE_FRAGMENTATION (compiles the fragment send/reassembly paths) and the +# reassembly cap RTPS_MAX_SAMPLE_SIZE (256 KB on the embedded profile). The +# facade's max payload size rises to this when fragmentation is enabled. +if(CONFIG_RTPS_ENABLE_FRAGMENTATION) + target_compile_definitions(${COMPONENT_LIB} PUBLIC + RTPS_ENABLE_FRAGMENTATION RTPS_MAX_SAMPLE_SIZE=262144) +endif() + diff --git a/components/rtps_embedded/Kconfig b/components/rtps_embedded/Kconfig new file mode 100644 index 000000000..aeb1f1f18 --- /dev/null +++ b/components/rtps_embedded/Kconfig @@ -0,0 +1,60 @@ +menu "RTPS (rtps_embedded)" + + choice RTPS_LIMITS_PROFILE + prompt "RTPS static limits profile" + default RTPS_LIMITS_PROFILE_EMBEDDED + help + Selects the compile-time capacity limits (profile header) used by the + rtps_embedded engine. Only the capacity caps differ between profiles. + The storage MODEL (static vs dynamic) is a separate knob + (RTPS_STORAGE_DYNAMIC below) and defaults to fully-static on ESP for + every profile - selecting a relaxed profile does NOT enable heap + storage. These caps are pure capacity limits and do NOT change any + bytes on the wire. + + config RTPS_LIMITS_PROFILE_EMBEDDED + bool "embedded (tight MCU caps)" + help + Tight caps sized for an ESP32-class MCU + (rtps/config_esp32.hpp). This is the default and the smallest + footprint. + + config RTPS_LIMITS_PROFILE_HOST + bool "host (relaxed static caps)" + help + Relaxed static caps suitable for small-to-medium DDS graphs + (rtps/config_desktop.hpp). Larger footprint than embedded. + + config RTPS_LIMITS_PROFILE_HOST_LARGE + bool "host_large (generous static caps)" + help + Generous static caps for large DDS graphs + (rtps/config_host_large.hpp). Significantly larger footprint; + only appropriate on targets with plenty of RAM. + endchoice + + config RTPS_STORAGE_DYNAMIC + bool "Use dynamic (heap) history/queue storage" + default n + help + By default the engine uses fully-static, zero-heap history and queue + storage on ESP targets (std::array), independent of the limits + profile above: when a history fills it drops the oldest sample. + Enable this to use heap-backed storage (std::deque) that instead + GROWS on demand to retain samples. Off by default so the MCU keeps + deterministic, zero-heap storage; enable only on targets that can + afford heap growth. Does not change any bytes on the wire. + + config RTPS_ENABLE_FRAGMENTATION + bool "Enable best-effort DATA_FRAG fragmentation" + default n + help + Enables sending and receiving samples larger than a single RTPS DATA + submessage (> ~64 KB) by splitting them into best-effort DATA_FRAG + submessages and reassembling them on receive (interoperates with + FastDDS / ROS 2). Default OFF so the MCU pays no code or memory cost + for it in the common (small-sample) case. When ON, reassembly is + bounded by RTPS_MAX_SAMPLE_SIZE (256 KB on the embedded profile) and + the participant's max payload size rises accordingly. + +endmenu diff --git a/components/rtps_embedded/include/rtps/common/types.hpp b/components/rtps_embedded/include/rtps/common/types.hpp index 68d1be8ca..03387af53 100644 --- a/components/rtps_embedded/include/rtps/common/types.hpp +++ b/components/rtps_embedded/include/rtps/common/types.hpp @@ -39,7 +39,11 @@ namespace rtps { // TODO move types to where they are needed! typedef uint16_t Ip4Port_t; -typedef uint16_t DataSize_t; +// Internal whole-sample / payload size type. Widened to 32-bit so a single +// sample can exceed 64 KB internally (prerequisite for DATA_FRAG). This is an +// INTERNAL type only: on-the-wire per-submessage length fields (e.g. +// SubmessageHeader::octetsToNextHeader) stay 16-bit per the RTPS spec. +typedef uint32_t DataSize_t; typedef int8_t ParticipantId_t; // With UDP only 120 possible enum class EntityKind_t : uint8_t { diff --git a/components/rtps_embedded/include/rtps/communication/EsppTransport.hpp b/components/rtps_embedded/include/rtps/communication/EsppTransport.hpp index 5664b4b0e..ecb2a952c 100644 --- a/components/rtps_embedded/include/rtps/communication/EsppTransport.hpp +++ b/components/rtps_embedded/include/rtps/communication/EsppTransport.hpp @@ -1,7 +1,6 @@ /* The MIT License -Copyright (c) 2019 Lehrstuhl Informatik 11 - RWTH Aachen University -Modifications Copyright (c) 2026 ATDev +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 @@ -18,9 +17,7 @@ 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 +This file is part of the espp embeddedRTPS port. */ #ifndef RTPS_ESPPTRANSPORT_H diff --git a/components/rtps_embedded/include/rtps/config.hpp b/components/rtps_embedded/include/rtps/config.hpp index 43abe3790..145e6172f 100644 --- a/components/rtps_embedded/include/rtps/config.hpp +++ b/components/rtps_embedded/include/rtps/config.hpp @@ -36,4 +36,16 @@ Author: i11 - Embedded Software, RWTH Aachen University #endif #endif +// Storage POLICY, orthogonal to the limits profile selected above. Dynamic +// (heap-backed, grow-on-full std::deque) storage is the default on host/PC +// builds; on ESP it is an explicit opt-in (Kconfig RTPS_STORAGE_DYNAMIC -> +// -DRTPS_STORAGE_DYNAMIC), so the MCU keeps zero-heap, deterministic history by +// default no matter which limits profile is chosen. The limits headers set +// capacity caps only and never enable dynamic storage by themselves. Define +// RTPS_STORAGE_STATIC to force static storage on a host build. Neither policy +// changes any bytes on the wire. +#if !defined(RTPS_STORAGE_DYNAMIC) && !defined(RTPS_STORAGE_STATIC) && !defined(ESP_PLATFORM) +#define RTPS_STORAGE_DYNAMIC +#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 index f480c72fb..36fca514d 100644 --- a/components/rtps_embedded/include/rtps/config_desktop.hpp +++ b/components/rtps_embedded/include/rtps/config_desktop.hpp @@ -32,6 +32,14 @@ namespace rtps { #define IS_LITTLE_ENDIAN 1 +// NOTE: this header sets capacity LIMITS only. The storage POLICY (static +// std::array vs heap-backed growable std::deque) is orthogonal and controlled +// centrally by RTPS_STORAGE_DYNAMIC, selected in config.hpp: dynamic by default +// on host/PC builds, explicit opt-in on ESP (Kconfig, default static). Selecting +// this profile therefore does NOT by itself switch storage to dynamic - which +// matters on ESP, where a relaxed limits profile must not silently enable heap +// storage. See storages/StorageArray.hpp. Capacity only - never touches wire bytes. + 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. @@ -40,26 +48,49 @@ const std::array IP_ADDRESS = {192, 168, 4, 1}; // Needs to be set i // participant share an identity, which breaks discovery between them. const GuidPrefix_t BASE_GUID_PREFIX = GUID_RANDOM; +// --------------------------------------------------------------------------- +// "host" limits profile (DEFAULT for non-ESP builds). +// +// RELAXED, fully-static capacity caps suitable for a compute host (laptop / +// Jetson / server) out-of-the-box. This is NOT a tiny profile: it is sized for +// real (small-to-medium) DDS graphs while keeping the deterministic, +// compile-time-static allocation model. For very large graphs select the +// "host_large" profile (config_host_large.hpp) via RTPS_LIMITS_PROFILE. +// +// NOTE: these are pure capacity caps and do NOT affect any bytes on the wire. +// --------------------------------------------------------------------------- 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; + +// Reassembly / large-sample cap (bytes). A sample larger than this is refused on +// publish and any partial reassembly exceeding it is dropped. On host this is the +// growable (Slice B deque) upper bound. Kept in sync with the facade's +// max_payload_size via the RTPS_MAX_SAMPLE_SIZE build macro when fragmentation is +// enabled. Capacity only - never touches wire bytes. +#ifndef RTPS_MAX_SAMPLE_SIZE +#define RTPS_MAX_SAMPLE_SIZE (8u * 1024u * 1024u) // 8 MB +#endif +const DataSize_t MAX_SAMPLE_SIZE = RTPS_MAX_SAMPLE_SIZE; + +const uint8_t MAX_NUM_PARTICIPANTS = 8; +const uint8_t NUM_STATELESS_WRITERS = 16; +const uint8_t NUM_STATELESS_READERS = 16; +const uint8_t NUM_STATEFUL_READERS = 32; +const uint8_t NUM_STATEFUL_WRITERS = 32; +const uint8_t NUM_WRITERS_PER_PARTICIPANT = 16; +const uint8_t NUM_READERS_PER_PARTICIPANT = 16; +const uint8_t NUM_WRITER_PROXIES_PER_READER = 8; +const uint8_t NUM_READER_PROXIES_PER_WRITER = 8; + +// uint16_t (not uint8_t): these bound SEDP MemoryPool<> sizes and the host +// value (256) already exceeds the 255 uint8_t range; host_large goes higher +// still. MemoryPool widens the value, so uint16_t is safe. +const uint16_t MAX_NUM_UNMATCHED_REMOTE_WRITERS = 256; +const uint16_t MAX_NUM_UNMATCHED_REMOTE_READERS = 128; + +const uint8_t MAX_NUM_READER_CALLBACKS = 8; const uint8_t HISTORY_SIZE_STATELESS = 2; -const uint8_t HISTORY_SIZE_STATEFUL = 10; +const uint8_t HISTORY_SIZE_STATEFUL = 16; const uint8_t MAX_TYPENAME_LENGTH = 64; const uint8_t MAX_TOPICNAME_LENGTH = 64; @@ -73,22 +104,22 @@ 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 uint8_t SPDP_MAX_NUMBER_FOUND_PARTICIPANTS = 32; +const uint8_t SPDP_MAX_NUM_LOCATORS = 8; 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 MAX_NUM_UDP_CONNECTIONS = 16; 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; +const int THREAD_POOL_WORKLOAD_QUEUE_LENGTH_USERTRAFFIC = 32; +const int THREAD_POOL_WORKLOAD_QUEUE_LENGTH_METATRAFFIC = 32; constexpr int OVERALL_HEAP_SIZE = THREAD_POOL_NUM_WRITERS * THREAD_POOL_WRITER_STACKSIZE + THREAD_POOL_NUM_READERS * THREAD_POOL_READER_STACKSIZE + diff --git a/components/rtps_embedded/include/rtps/config_esp32.hpp b/components/rtps_embedded/include/rtps/config_esp32.hpp index 53bfc6571..d1575dcb4 100644 --- a/components/rtps_embedded/include/rtps/config_esp32.hpp +++ b/components/rtps_embedded/include/rtps/config_esp32.hpp @@ -40,6 +40,15 @@ const std::array IP_ADDRESS = {192, 168, 4, 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 + +// Reassembly / large-sample cap (bytes) when RTPS_ENABLE_FRAGMENTATION is opted +// in on this MCU (Kconfig, default off). Bounded (256 KB) - a fragmented sample +// larger than this is refused/dropped. Unused when fragmentation is off. +#ifndef RTPS_MAX_SAMPLE_SIZE +#define RTPS_MAX_SAMPLE_SIZE (256u * 1024u) // 256 KB +#endif +const DataSize_t MAX_SAMPLE_SIZE = RTPS_MAX_SAMPLE_SIZE; + const uint8_t NUM_STATELESS_WRITERS = 5; const uint8_t NUM_STATELESS_READERS = 5; const uint8_t NUM_STATEFUL_READERS = 5; diff --git a/components/rtps_embedded/include/rtps/config_host_large.hpp b/components/rtps_embedded/include/rtps/config_host_large.hpp new file mode 100644 index 000000000..09099a64d --- /dev/null +++ b/components/rtps_embedded/include/rtps/config_host_large.hpp @@ -0,0 +1,125 @@ +/* +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_HOST_LARGE_H +#define RTPS_CONFIG_HOST_LARGE_H + +#include "rtps/common/types.hpp" + +namespace rtps { + +#define IS_LITTLE_ENDIAN 1 + +// NOTE: this header sets capacity LIMITS only. The storage POLICY (static +// std::array vs heap-backed growable std::deque) is orthogonal and controlled +// centrally by RTPS_STORAGE_DYNAMIC, selected in config.hpp: dynamic by default +// on host/PC builds, explicit opt-in on ESP (Kconfig, default static). Selecting +// this profile therefore does NOT by itself switch storage to dynamic - which +// matters on ESP, where a relaxed limits profile must not silently enable heap +// storage. See storages/StorageArray.hpp. Capacity only - never touches wire bytes. + +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; + +// --------------------------------------------------------------------------- +// "host_large" limits profile (opt-in via RTPS_LIMITS_PROFILE=host_large). +// +// GENEROUS, fully-static capacity caps for large DDS graphs (big ROS 2 systems) +// on a compute host with plenty of RAM. Same deterministic, compile-time-static +// allocation model as the other profiles - just sized an order of magnitude +// larger than "host" (config_desktop.hpp). These are pure capacity caps and do +// NOT affect any bytes on the wire. +// --------------------------------------------------------------------------- +const uint8_t DOMAIN_ID = 0; // 230 possible with UDP + +// Reassembly / large-sample cap (bytes). See config_desktop.hpp. Capacity only. +#ifndef RTPS_MAX_SAMPLE_SIZE +#define RTPS_MAX_SAMPLE_SIZE (8u * 1024u * 1024u) // 8 MB +#endif +const DataSize_t MAX_SAMPLE_SIZE = RTPS_MAX_SAMPLE_SIZE; + +const uint8_t MAX_NUM_PARTICIPANTS = 32; +const uint8_t NUM_STATELESS_WRITERS = 64; +const uint8_t NUM_STATELESS_READERS = 64; +const uint8_t NUM_STATEFUL_READERS = 128; +const uint8_t NUM_STATEFUL_WRITERS = 128; +const uint8_t NUM_WRITERS_PER_PARTICIPANT = 64; +const uint8_t NUM_READERS_PER_PARTICIPANT = 64; +const uint8_t NUM_WRITER_PROXIES_PER_READER = 16; +const uint8_t NUM_READER_PROXIES_PER_WRITER = 16; + +// uint16_t (not uint8_t): these bound SEDP MemoryPool<> sizes and the values +// here (1024 / 512) far exceed the 255 uint8_t range. +// MemoryPool widens the value, so uint16_t is safe. +const uint16_t MAX_NUM_UNMATCHED_REMOTE_WRITERS = 1024; +const uint16_t MAX_NUM_UNMATCHED_REMOTE_READERS = 512; + +const uint8_t MAX_NUM_READER_CALLBACKS = 16; + +const uint8_t HISTORY_SIZE_STATELESS = 2; +const uint8_t HISTORY_SIZE_STATEFUL = 32; + +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 = 128; +const uint8_t SPDP_MAX_NUM_LOCATORS = 16; +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 = 32; + +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 = 64; +const int THREAD_POOL_WORKLOAD_QUEUE_LENGTH_METATRAFFIC = 64; + +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_HOST_LARGE_H diff --git a/components/rtps_embedded/include/rtps/entities/Reader.hpp b/components/rtps_embedded/include/rtps/entities/Reader.hpp index 96e1558a8..8e5e169f0 100644 --- a/components/rtps_embedded/include/rtps/entities/Reader.hpp +++ b/components/rtps_embedded/include/rtps/entities/Reader.hpp @@ -34,6 +34,9 @@ Author: i11 - Embedded Software, RWTH Aachen University #include "rtps/storages/MemoryPool.hpp" #include #include +#ifdef RTPS_ENABLE_FRAGMENTATION +#include +#endif namespace rtps { @@ -113,6 +116,18 @@ class Reader : public espp::BaseComponent { virtual bool sendPreemptiveAckNack(const WriterProxy &writer); +#ifdef RTPS_ENABLE_FRAGMENTATION + /// Accumulate one DATA_FRAG fragment (best-effort reassembly). When all + /// fragments of the sample identified by (writerGuid, sn) have arrived, the + /// completed sample is delivered through the normal newChange() path (same as + /// a single DATA). A newer sample evicts an older incomplete one; samples + /// larger than Config::MAX_SAMPLE_SIZE are refused. Thread-safe. + void newFragment(const Guid_t &writerGuid, const SequenceNumber_t &sn, + uint32_t fragmentStartingNum, uint16_t fragmentsInSubmessage, + uint16_t fragmentSize, uint32_t sampleSize, const uint8_t *fragData, + DataSize_t fragDataLen); +#endif + protected: void executeCallbacks(const ReaderCacheChange &cacheChange); bool initMutex(); @@ -140,6 +155,25 @@ class Reader : public espp::BaseComponent { // Guards manipulation of callback array std::recursive_mutex m_callback_mutex; + +#ifdef RTPS_ENABLE_FRAGMENTATION + // Single best-effort reassembly slot: accumulates the fragments of one sample + // at a time (a newer sample or different writer evicts an older incomplete + // one). Bounded by Config::MAX_SAMPLE_SIZE. + struct Reassembly { + bool active = false; + Guid_t writerGuid{}; + SequenceNumber_t sn{}; + uint32_t sampleSize = 0; + uint16_t fragmentSize = 0; + uint32_t totalFragments = 0; + uint32_t receivedFragments = 0; + std::vector buffer; + std::vector received; + }; + Reassembly m_reassembly; + std::mutex m_reassembly_mutex; +#endif }; } // namespace rtps diff --git a/components/rtps_embedded/include/rtps/entities/StatefulWriter.hpp b/components/rtps_embedded/include/rtps/entities/StatefulWriter.hpp index e972127ce..743b0325f 100644 --- a/components/rtps_embedded/include/rtps/entities/StatefulWriter.hpp +++ b/components/rtps_embedded/include/rtps/entities/StatefulWriter.hpp @@ -98,6 +98,12 @@ class StatefulWriter final : public Writer { bool sendData(const ReaderProxy &reader, const CacheChange *next); bool sendDataWRMulticast(const ReaderProxy &reader, const CacheChange *next); +#ifdef RTPS_ENABLE_FRAGMENTATION + // Split next->data into DATA_FRAG submessages (one per datagram) and send them + // to destAddr:destPort. Used when a sample is too large for a single DATA. + bool sendSampleFragmented(const Ip4AddressBytes &destAddr, Ip4Port_t destPort, + const EntityId_t &readerId, const CacheChange *next); +#endif void sendHeartBeat(); void sendGap(const ReaderProxy &reader, const SequenceNumber_t &firstMissing, const SequenceNumber_t &nextValid); diff --git a/components/rtps_embedded/include/rtps/entities/StatelessWriter.hpp b/components/rtps_embedded/include/rtps/entities/StatelessWriter.hpp index ba1b1d344..eb2f4d88f 100644 --- a/components/rtps_embedded/include/rtps/entities/StatelessWriter.hpp +++ b/components/rtps_embedded/include/rtps/entities/StatelessWriter.hpp @@ -55,6 +55,13 @@ class StatelessWriter : public Writer { void reset() override; private: +#ifdef RTPS_ENABLE_FRAGMENTATION + // Split next->data into DATA_FRAG submessages (one per datagram) to + // destAddr:destPort. Used when a sample is too large for a single DATA. + bool sendSampleFragmented(const Ip4AddressBytes &destAddr, Ip4Port_t destPort, + const EntityId_t &readerId, const CacheChange *next); +#endif + EsppTransport *m_transport; SimpleHistoryCache m_history; diff --git a/components/rtps_embedded/include/rtps/entities/Writer.hpp b/components/rtps_embedded/include/rtps/entities/Writer.hpp index eb3ba1c31..966a5bf97 100644 --- a/components/rtps_embedded/include/rtps/entities/Writer.hpp +++ b/components/rtps_embedded/include/rtps/entities/Writer.hpp @@ -82,10 +82,20 @@ class Writer : public espp::BaseComponent { bool isBuiltinEndpoint(); + /// Set the nominal per-fragment payload size used when a published sample is + /// too large for a single DATA submessage and must be split into DATA_FRAG + /// submessages. Clamped to <= MAX_FRAGMENT_SIZE so each single-fragment + /// DATA_FRAG still fits one UDP datagram. No effect when fragmentation is + /// compiled out. + void setFragmentSize(uint16_t fragmentSize) { m_fragmentSize = fragmentSize; } + protected: Writer(); SequenceNumber_t m_sedp_sequence_number; + //! Nominal per-fragment payload size for DATA_FRAG (default 63000). + uint16_t m_fragmentSize = 63000; + std::recursive_mutex m_mutex; Ip4Port_t m_srcPort; diff --git a/components/rtps_embedded/include/rtps/messages/MessageFactory.hpp b/components/rtps_embedded/include/rtps/messages/MessageFactory.hpp index 0507b6fec..ccdec7b8f 100644 --- a/components/rtps_embedded/include/rtps/messages/MessageFactory.hpp +++ b/components/rtps_embedded/include/rtps/messages/MessageFactory.hpp @@ -108,8 +108,13 @@ void addSubMessageData(Buffer &buffer, const PayloadBuffer &filledPayload, bool msg.header.flags = FLAG_BIG_ENDIAN; #endif - msg.header.octetsToNextHeader = - SubmessageData::getRawSize() + filledPayload.spaceUsed() - numBytesUntilEndOfLength; + // octetsToNextHeader is a 16-bit wire field. spaceUsed() is now DataSize_t + // (uint32) but the non-fragmented DATA path is only reached for samples that + // fit a single submessage (< 64 KB), so the narrowing to uint16 is correct + // and required for wire-format neutrality. (DATA_FRAG, when it lands, will + // carry oversized samples via fragment-sized submessages instead.) + msg.header.octetsToNextHeader = static_cast( + SubmessageData::getRawSize() + filledPayload.spaceUsed() - numBytesUntilEndOfLength); if (containsInlineQos) { msg.header.flags |= FLAG_INLINE_QOS; @@ -133,6 +138,45 @@ void addSubMessageData(Buffer &buffer, const PayloadBuffer &filledPayload, bool } } +#ifdef RTPS_ENABLE_FRAGMENTATION +// Append one DATA_FRAG submessage carrying [fragData, fragData+fragLen) as the +// serializedData for fragment(s) starting at fragStartNum (1-based). fragLen must +// be <= 65507 - overhead (guaranteed because callers pass fragmentSize-bounded +// chunks). fragmentsInSubmessage is normally 1 (simplest, interop-friendly). +template +void addSubMessageDataFrag(Buffer &buffer, const uint8_t *fragData, uint16_t fragLen, + uint32_t fragStartNum, uint16_t fragsInSubmsg, uint16_t fragmentSize, + uint32_t sampleSize, const SequenceNumber_t &SN, + const EntityId_t &writerID, const EntityId_t &readerID) { + SubmessageDataFrag msg; + msg.header.submessageId = SubmessageKind::DATA_FRAG; +#if IS_LITTLE_ENDIAN + msg.header.flags = FLAG_LITTLE_ENDIAN; +#else + msg.header.flags = FLAG_BIG_ENDIAN; +#endif + // DATA_FRAG has no separate "data present" flag (bit 2 is the Key flag); the + // serializedData is always present. Alive CDR samples set only E (+ Q if + // inlineQos, which we do not emit). + msg.header.octetsToNextHeader = + static_cast(SubmessageDataFrag::getRawSize() + fragLen - numBytesUntilEndOfLength); + msg.extraFlags = 0; + msg.octetsToInlineQos = SubmessageDataFrag::octetsToInlineQosValue(); + msg.readerId = readerID; + msg.writerId = writerID; + msg.writerSN = SN; + msg.fragmentStartingNum = fragStartNum; + msg.fragmentsInSubmessage = fragsInSubmsg; + msg.fragmentSize = fragmentSize; + msg.sampleSize = sampleSize; + + serializeMessage(buffer, msg); + + buffer.reserve(fragLen); + buffer.append(fragData, fragLen); +} +#endif // RTPS_ENABLE_FRAGMENTATION + template void addHeartbeat(Buffer &buffer, EntityId_t writerId, EntityId_t readerId, SequenceNumber_t firstSN, SequenceNumber_t lastSN, Count_t count) { diff --git a/components/rtps_embedded/include/rtps/messages/MessageReceiver.hpp b/components/rtps_embedded/include/rtps/messages/MessageReceiver.hpp index c0ec3e78d..c0655b49c 100644 --- a/components/rtps_embedded/include/rtps/messages/MessageReceiver.hpp +++ b/components/rtps_embedded/include/rtps/messages/MessageReceiver.hpp @@ -68,6 +68,11 @@ class MessageReceiver : public espp::BaseComponent { const MessageSourceState &sourceState); bool processDataSubmessage(MessageProcessingInfo &msgInfo, const SubmessageHeader &submsgHeader, const MessageSourceState &sourceState); +#ifdef RTPS_ENABLE_FRAGMENTATION + bool processDataFragSubmessage(MessageProcessingInfo &msgInfo, + const SubmessageHeader &submsgHeader, + const MessageSourceState &sourceState); +#endif bool processHeartbeatSubmessage(MessageProcessingInfo &msgInfo, const MessageSourceState &sourceState); bool processAckNackSubmessage(MessageProcessingInfo &msgInfo, diff --git a/components/rtps_embedded/include/rtps/messages/MessageTypes.hpp b/components/rtps_embedded/include/rtps/messages/MessageTypes.hpp index 0f2a84367..fd76c7072 100644 --- a/components/rtps_embedded/include/rtps/messages/MessageTypes.hpp +++ b/components/rtps_embedded/include/rtps/messages/MessageTypes.hpp @@ -206,6 +206,58 @@ struct SubmessageHeartbeat { } }; +// DATA_FRAG submessage (SubmessageKind 0x16). Carries one (or more, packed) +// fragment(s) of a serialized sample that is too large to fit a single DATA +// submessage. See OMG DDSI-RTPS 2.3+, section 8.3.7.3. All the whole-sample size +// bookkeeping (fragmentStartingNum is 1-based, sampleSize is the total serialized +// sample size incl. the CDR encapsulation header); fragmentSize is the fixed +// nominal per-fragment byte count (the last fragment is shorter). +struct SubmessageDataFrag { + SubmessageHeader header; + uint16_t extraFlags; + uint16_t octetsToInlineQos; + EntityId_t readerId; + EntityId_t writerId; + SequenceNumber_t writerSN; + uint32_t fragmentStartingNum; // 1-based index of the first fragment here + uint16_t fragmentsInSubmessage; + uint16_t fragmentSize; + uint32_t sampleSize; // total serialized size of the whole sample + static constexpr uint16_t getRawSize() { + return SubmessageHeader::getRawSize() + sizeof(uint16_t) + sizeof(uint16_t) // extra/octetsToIQ + + (2 * 3 + 2 * 1) // 2 EntityIDs + + sizeof(SequenceNumber_t) // writerSN + + sizeof(uint32_t) + sizeof(uint16_t) + sizeof(uint16_t) + + sizeof(uint32_t); // fragmentStartingNum + fragmentsInSubmessage + fragmentSize + + // sampleSize + } + // octetsToInlineQos value: bytes from the field after octetsToInlineQos up to + // the first byte of inlineQos/serializedData (readerId+writerId+writerSN + + // fragmentStartingNum+fragmentsInSubmessage+fragmentSize+sampleSize). + static constexpr uint16_t octetsToInlineQosValue() { + return (2 * 3 + 2 * 1) + sizeof(SequenceNumber_t) + sizeof(uint32_t) + sizeof(uint16_t) + + sizeof(uint16_t) + sizeof(uint32_t); + } +}; + +// Largest serialized sample payload that still fits one unfragmented DATA +// submessage inside a single UDP datagram (max UDP payload 65507 - RTPS header +// 20 - INFO_TS 12 - DATA submessage header 24). Samples larger than this are +// sent as DATA_FRAG; smaller ones keep the byte-identical single-DATA path. +static constexpr DataSize_t MAX_UNFRAGMENTED_PAYLOAD = + 65507 - Header::getRawSize() - (SubmessageHeader::getRawSize() + sizeof(Time_t)) - + SubmessageData::getRawSize(); + +// Largest per-fragment payload that keeps a single-fragment DATA_FRAG submessage +// within one UDP datagram (max UDP payload 65507 - RTPS header 20 - INFO_TS 12 - +// DATA_FRAG submessage header 36). DATA_FRAG's fixed header is 12 bytes larger +// than DATA's, so this bound is 12 bytes below MAX_UNFRAGMENTED_PAYLOAD (65439 vs +// 65451). Configured fragment sizes are clamped to this so a fragment can never +// build an oversized datagram whose send would silently fail. +static constexpr DataSize_t MAX_FRAGMENT_SIZE = 65507 - Header::getRawSize() - + (SubmessageHeader::getRawSize() + sizeof(Time_t)) - + SubmessageDataFrag::getRawSize(); + struct SubmessageInfoDST { SubmessageHeader header; GuidPrefix_t guidPrefix; @@ -306,6 +358,28 @@ template bool serializeMessage(Buffer &buffer, SubmessageData return true; } +template bool serializeMessage(Buffer &buffer, SubmessageDataFrag &msg) { + if (!buffer.reserve(SubmessageDataFrag::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)); + buffer.append(reinterpret_cast(&msg.fragmentStartingNum), sizeof(uint32_t)); + buffer.append(reinterpret_cast(&msg.fragmentsInSubmessage), sizeof(uint16_t)); + buffer.append(reinterpret_cast(&msg.fragmentSize), sizeof(uint16_t)); + buffer.append(reinterpret_cast(&msg.sampleSize), sizeof(uint32_t)); + return true; +} + template bool serializeMessage(Buffer &buffer, SubmessageHeartbeat &msg) { if (!buffer.reserve(SubmessageHeartbeat::getRawSize())) { return false; @@ -397,6 +471,10 @@ bool deserializeMessage(const MessageProcessingInfo &info, SubmessageHeader &hea bool deserializeMessage(const MessageProcessingInfo &info, SubmessageData &msg); +#ifdef RTPS_ENABLE_FRAGMENTATION +bool deserializeMessage(const MessageProcessingInfo &info, SubmessageDataFrag &msg); +#endif + bool deserializeMessage(const MessageProcessingInfo &info, SubmessageHeartbeat &msg); bool deserializeMessage(const MessageProcessingInfo &info, SubmessageAckNack &msg); diff --git a/components/rtps_embedded/include/rtps/storages/HistoryCacheWithDeletion.hpp b/components/rtps_embedded/include/rtps/storages/HistoryCacheWithDeletion.hpp index 7657a6b75..72545d6ec 100644 --- a/components/rtps_embedded/include/rtps/storages/HistoryCacheWithDeletion.hpp +++ b/components/rtps_embedded/include/rtps/storages/HistoryCacheWithDeletion.hpp @@ -27,8 +27,13 @@ Author: i11 - Embedded Software, RWTH Aachen University #define HISTORYCACHEWITHDELETION_H #include +#include #include +#include "rtps/config.hpp" +#include "rtps/storages/CacheChange.hpp" +#include "rtps/storages/StorageArray.hpp" + namespace rtps { /** @@ -63,6 +68,14 @@ template class HistoryCacheWithDeletion { m_dispose_after_write_cnt++; } +#ifdef RTPS_STORAGE_DYNAMIC + // Host: grow (retain) instead of dropping the oldest change when full. Done + // before capturing `place` so growth cannot invalidate the pointer. + if (isFull()) { + grow(); + } +#endif + CacheChange *place = &m_buffer[m_head]; incrementHead(); @@ -204,13 +217,46 @@ template class HistoryCacheWithDeletion { } private: - std::array m_buffer{}; + // Static path: fixed std::array (byte-identical to + // before). Dynamic path: heap-backed, reserved to SIZE + 1, grown by grow(). + StorageArray 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}; +#ifdef RTPS_STORAGE_DYNAMIC + // Host only: double the ring capacity, then re-linearize the live changes so + // the head/tail indices remain valid after the (append-only) resize. The live + // range [m_tail, m_head) is move-assigned into the freshly grown region + // [oldCap, ...) in logical order. Uses move-assignment only (CacheChange is + // move-assignable but not move-constructible). + void grow() { + const std::size_t oldCap = m_buffer.size(); + // 16-bit ring indices: refuse to grow past what they can represent, else the + // m_head/m_tail casts below truncate and corrupt the ring. A history cache + // never legitimately needs this many entries; it then behaves as full + // (drop-oldest) at this bound. + if (oldCap * 2 > std::numeric_limits::max()) { + return; + } + m_buffer.ensureSize(oldCap * 2); + std::size_t dst = oldCap; + uint16_t src = m_tail; + while (src != m_head) { + m_buffer[dst] = std::move(m_buffer[src]); + ++dst; + ++src; + if (src >= oldCap) { + src = 0; // wrap at the OLD capacity + } + } + m_tail = static_cast(oldCap); + m_head = static_cast(dst); + } +#endif + bool getChangeBySN(const SequenceNumber_t &sn, CacheChange **out_change, uint16_t &out_buffer_position) { if (!isSNInRange(sn)) { diff --git a/components/rtps_embedded/include/rtps/storages/MemoryPool.hpp b/components/rtps_embedded/include/rtps/storages/MemoryPool.hpp index 0bfa1e93c..51dcf1488 100644 --- a/components/rtps_embedded/include/rtps/storages/MemoryPool.hpp +++ b/components/rtps_embedded/include/rtps/storages/MemoryPool.hpp @@ -30,6 +30,8 @@ Author: i11 - Embedded Software, RWTH Aachen University #include #include +#include "rtps/storages/StorageArray.hpp" + namespace rtps { template class MemoryPool { @@ -42,10 +44,16 @@ template class MemoryPool { using pointer = IT_TYPE *; using reference = IT_TYPE &; + // The bitmap is snapshotted so removals during iteration operate on stable + // bits. On the static path this StorageArray is a fixed std::array (a plain + // copy, exactly as the previous memcpy). On the dynamic path it is a vector + // sized to the pool's current (possibly grown) bitmap. The pool's capacity + // does not change during a single iteration, so m_pool->capacity() is a + // stable end sentinel on both paths (and folds to the constant SIZE on the + // static path). explicit MemoryPoolIterator(MemoryPool &pool) - : m_pool(&pool) { - memcpy(m_bitMap, m_pool->m_bitMap, sizeof(m_bitMap)); - } + : m_pool(&pool) + , m_bitMap(pool.m_bitMap) {} bool operator==(const MemoryPoolIterator &other) const { return m_bit == other.m_bit; } @@ -58,14 +66,14 @@ template class MemoryPool { // Pre-increment MemoryPoolIterator &operator++() { if (m_pool->m_numElements == 0) { - m_bit = SIZE; + m_bit = m_pool->capacity(); 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); + } while (!(m_bitMap[bucket] & (1 << (m_bit % 8))) && m_bit < m_pool->capacity()); return *this; } @@ -80,7 +88,7 @@ template class MemoryPool { private: friend class MemoryPool; MemoryPool *m_pool; - uint8_t m_bitMap[SIZE / 8 + 1]; + StorageArray m_bitMap; uint32_t m_bit = 0; }; @@ -89,9 +97,14 @@ template class MemoryPool { typedef bool (*condition_fp)(TYPE); - uint32_t getSize() { return SIZE; } + // Current capacity. On the static path capacity() folds to the constant SIZE + // (StorageArray::size() is constexpr), so all uses below generate identical + // code to the original `SIZE`. On the dynamic path it tracks the grown size. + uint32_t capacity() const { return static_cast(m_data.size()); } + + uint32_t getSize() const { return capacity(); } - bool isFull() const { return m_numElements == SIZE; } + bool isFull() const { return m_numElements == capacity(); } bool isEmpty() const { return m_numElements == 0; } @@ -99,10 +112,15 @@ template class MemoryPool { bool add(const TYPE &data) { if (isFull()) { +#ifdef RTPS_STORAGE_DYNAMIC + // Host: grow past the profile cap instead of hard-failing. + grow(); +#else printf("[MemoryPool] RESSOURCE LIMIT EXCEEDED \n"); return false; +#endif } - for (uint32_t bucket = 0; bucket < sizeof(m_bitMap); ++bucket) { + for (uint32_t bucket = 0; bucket < m_bitMap.size(); ++bucket) { if (m_bitMap[bucket] != 0xFF) { uint8_t byte = m_bitMap[bucket]; for (uint8_t bit = 0; bit < 8; ++bit) { @@ -146,7 +164,7 @@ template class MemoryPool { } void clear() { - for (unsigned int i = 0; i < (SIZE / 8 + 1); i++) { + for (unsigned int i = 0; i < m_bitMap.size(); i++) { m_bitMap[i] = 0; } m_numElements = 0; @@ -171,14 +189,28 @@ template class MemoryPool { MemPoolIter end() { MemPoolIter endIt(*this); - endIt.m_bit = SIZE; + endIt.m_bit = capacity(); return endIt; } private: - uint8_t m_bitMap[SIZE / 8 + 1]{}; +#ifdef RTPS_STORAGE_DYNAMIC + // Host only: double capacity, growing the data array and its bitmap in + // lockstep. New bitmap bytes are zero-initialized (all slots free) by the + // vector resize, so the free-slot scan in add() finds room immediately. + void grow() { + const uint32_t newCap = capacity() * 2; + m_data.ensureSize(newCap); + m_bitMap.ensureSize(newCap / 8 + 1); + } +#endif + + // Static path: fixed std::array (byte-identical footprint to the previous raw + // `uint8_t[SIZE/8+1]` / `TYPE[SIZE]`). Dynamic path: heap-backed, reserved to + // SIZE, grown by grow(). The two arrays always grow in lockstep. + StorageArray m_bitMap; uint32_t m_numElements = 0; - TYPE m_data[SIZE]; + StorageArray m_data; }; } // namespace rtps diff --git a/components/rtps_embedded/include/rtps/storages/PayloadBuffer.hpp b/components/rtps_embedded/include/rtps/storages/PayloadBuffer.hpp index 0637837dc..581aa8007 100644 --- a/components/rtps_embedded/include/rtps/storages/PayloadBuffer.hpp +++ b/components/rtps_embedded/include/rtps/storages/PayloadBuffer.hpp @@ -1,7 +1,6 @@ /* The MIT License -Copyright (c) 2019 Lehrstuhl Informatik 11 - RWTH Aachen University -Modifications Copyright (c) 2026 ATDev +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 @@ -18,9 +17,7 @@ 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 +This file is part of the espp embeddedRTPS port. */ #ifndef RTPS_PAYLOADBUFFER_H diff --git a/components/rtps_embedded/include/rtps/storages/SimpleHistoryCache.hpp b/components/rtps_embedded/include/rtps/storages/SimpleHistoryCache.hpp index 540560dbc..06ed0371b 100644 --- a/components/rtps_embedded/include/rtps/storages/SimpleHistoryCache.hpp +++ b/components/rtps_embedded/include/rtps/storages/SimpleHistoryCache.hpp @@ -28,6 +28,8 @@ Author: i11 - Embedded Software, RWTH Aachen University #include "rtps/config.hpp" #include "rtps/storages/CacheChange.hpp" +#include "rtps/storages/StorageArray.hpp" +#include namespace rtps { @@ -58,6 +60,14 @@ template class SimpleHistoryCache { change.data.append(data, size); change.sequenceNumber = ++m_lastUsedSequenceNumber; +#ifdef RTPS_STORAGE_DYNAMIC + // Host: grow (retain) instead of dropping the oldest change when full. Done + // before capturing `place` so growth cannot invalidate the pointer. + if (isFull()) { + grow(); + } +#endif + CacheChange *place = &m_buffer[m_head]; incrementHead(); @@ -137,13 +147,46 @@ template class SimpleHistoryCache { } private: - std::array m_buffer{}; + // Static path: fixed std::array (byte-identical to + // before). Dynamic path: heap-backed, reserved to SIZE + 1, grown by grow(). + StorageArray 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}; +#ifdef RTPS_STORAGE_DYNAMIC + // Host only: double the ring capacity, then re-linearize the live changes so + // the head/tail indices remain valid after the (append-only) resize. The live + // range [m_tail, m_head) is move-assigned into the freshly grown region + // [oldCap, ...) in logical order. Uses move-assignment only (CacheChange is + // move-assignable but not move-constructible). + void grow() { + const std::size_t oldCap = m_buffer.size(); + // 16-bit ring indices: refuse to grow past what they can represent, else the + // m_head/m_tail casts below truncate and corrupt the ring. A history cache + // never legitimately needs this many entries; it then behaves as full + // (drop-oldest) at this bound. + if (oldCap * 2 > std::numeric_limits::max()) { + return; + } + m_buffer.ensureSize(oldCap * 2); + std::size_t dst = oldCap; + uint16_t src = m_tail; + while (src != m_head) { + m_buffer[dst] = std::move(m_buffer[src]); + ++dst; + ++src; + if (src >= oldCap) { + src = 0; // wrap at the OLD capacity + } + } + m_tail = static_cast(oldCap); + m_head = static_cast(dst); + } +#endif + inline void incrementHead() { incrementIterator(m_head); if (m_head == m_tail) { diff --git a/components/rtps_embedded/include/rtps/storages/StorageArray.hpp b/components/rtps_embedded/include/rtps/storages/StorageArray.hpp new file mode 100644 index 000000000..e0aa311b7 --- /dev/null +++ b/components/rtps_embedded/include/rtps/storages/StorageArray.hpp @@ -0,0 +1,123 @@ +/* +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 the espp embeddedRTPS port. +*/ + +#ifndef RTPS_STORAGE_ARRAY_H +#define RTPS_STORAGE_ARRAY_H + +// --------------------------------------------------------------------------- +// StorageArray: the compile-time storage policy for the engine's VALUE +// pools (MemoryPool, SimpleHistoryCache, HistoryCacheWithDeletion, +// ThreadSafeCircularBuffer). +// +// The policy is selected purely at compile time by the limits profile header +// (rtps/config.hpp -> config_*.hpp): +// +// * ESP32 / "embedded" profile -> RTPS_STORAGE_DYNAMIC is NOT defined +// => backed by a fixed std::array. This is byte-identical, in +// behaviour and footprint, to the raw C arrays the engine shipped +// with. No heap, no runtime capacity, no growth path is compiled in. +// Determinism for the MCU is preserved. +// +// * host / host_large profiles -> RTPS_STORAGE_DYNAMIC IS defined +// => backed by a std::vector reserved (sized) to N up front, that +// can grow past the profile cap instead of hard-failing when a pool +// fills. Heap-backed, non-deterministic, host-only. +// +// StorageArray only stores capacity; it never touches serialized bytes, so the +// golden wire tests stay byte-identical on both paths. +// +// The surface is intentionally minimal - exactly what the pools need: +// operator[](i), size(), and (dynamic only) ensureSize(n) to grow. +// --------------------------------------------------------------------------- + +#include +#include + +#include "rtps/config.hpp" // pulls in the profile header that (does not) define + // RTPS_STORAGE_DYNAMIC + +#ifdef RTPS_STORAGE_DYNAMIC +#include +#endif + +namespace rtps { + +#ifdef RTPS_STORAGE_DYNAMIC + +// Dynamic (host) storage: heap-backed, sized (reserved) to N up front, growable +// past N. +// +// Backed by std::deque rather than std::vector deliberately: some element types +// stored here (notably CacheChange) are move-ASSIGNABLE but not +// move-CONSTRUCTIBLE, so a std::vector could not relocate them when it grows. +// std::deque grows by appending default-constructed elements and never +// relocates the existing ones, so resize() needs only DefaultConstructible and +// previously handed-out indices/references stay valid. Callers that keep ring +// indices (the history caches / circular buffer) re-linearize themselves after +// ensureSize() using move-assignment only. +template class StorageArray { +public: + StorageArray() + : m_data(N) {} // initial size N (value-initialized) == "reserved to N" + + T &operator[](std::size_t i) { return m_data[i]; } + const T &operator[](std::size_t i) const { return m_data[i]; } + + std::size_t size() const { return m_data.size(); } + + /// Grow so that at least new_size elements are addressable. Existing elements + /// are preserved in place; freshly added elements are value-initialized. Only + /// ever grows (never shrinks). + void ensureSize(std::size_t new_size) { + if (new_size > m_data.size()) { + m_data.resize(new_size); + } + } + +private: + std::deque m_data; +}; + +#else + +// Static (embedded) storage: fixed array, no heap, no growth. Zero-overhead; +// codegen is identical to the raw `T m_data[N]` the engine used before. +template class StorageArray { +public: + T &operator[](std::size_t i) { return m_data[i]; } + const T &operator[](std::size_t i) const { return m_data[i]; } + + static constexpr std::size_t size() { return N; } + + // ensureSize() intentionally does not exist on the static path: any attempt + // to grow a fixed pool is a compile error, guaranteeing no growth code is + // reachable on the MCU. + +private: + std::array m_data{}; +}; + +#endif // RTPS_STORAGE_DYNAMIC + +} // namespace rtps + +#endif // RTPS_STORAGE_ARRAY_H diff --git a/components/rtps_embedded/include/rtps/storages/ThreadSafeCircularBuffer.hpp b/components/rtps_embedded/include/rtps/storages/ThreadSafeCircularBuffer.hpp index fb0b95f15..2171bb976 100644 --- a/components/rtps_embedded/include/rtps/storages/ThreadSafeCircularBuffer.hpp +++ b/components/rtps_embedded/include/rtps/storages/ThreadSafeCircularBuffer.hpp @@ -31,6 +31,8 @@ Author: i11 - Embedded Software, RWTH Aachen University #include #include +#include "rtps/storages/StorageArray.hpp" + namespace rtps { template class ThreadSafeCircularBuffer { @@ -53,7 +55,9 @@ template class ThreadSafeCircularBuffer { void clear(); private: - std::array m_buffer{}; + // Static path: fixed std::array (byte-identical to before). + // Dynamic path: heap-backed, reserved to SIZE + 1, grown by grow(). + StorageArray m_buffer; uint16_t m_head = 0; uint16_t m_tail = 0; uint32_t m_num_elements = 0; @@ -67,6 +71,10 @@ template class ThreadSafeCircularBuffer { inline void incrementIterator(uint16_t &iterator) const; inline void incrementTail(); inline void incrementHead(); +#ifdef RTPS_STORAGE_DYNAMIC + // Host only: double the ring capacity (caller holds m_mutex). + bool grow(); // returns false if the ring cannot grow further (16-bit indices) +#endif }; } // namespace rtps diff --git a/components/rtps_embedded/include/rtps/storages/ThreadSafeCircularBuffer.tpp b/components/rtps_embedded/include/rtps/storages/ThreadSafeCircularBuffer.tpp index 35c1bd913..b46ef824b 100644 --- a/components/rtps_embedded/include/rtps/storages/ThreadSafeCircularBuffer.tpp +++ b/components/rtps_embedded/include/rtps/storages/ThreadSafeCircularBuffer.tpp @@ -7,27 +7,39 @@ 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 { + if (isFull()) { +#ifdef RTPS_STORAGE_DYNAMIC + if (!grow()) { // host: grow instead of dropping - unless at the index limit + m_insertion_failures++; + return false; + } +#else m_insertion_failures++; return false; +#endif } + m_buffer[m_head] = std::move(elem); + incrementHead(); + return true; } template bool ThreadSafeCircularBuffer::copyElementIntoBuffer(const T &elem) { std::lock_guard lock(m_mutex); - if (!isFull()) { - m_buffer[m_head] = elem; - incrementHead(); - return true; - } else { + if (isFull()) { +#ifdef RTPS_STORAGE_DYNAMIC + if (!grow()) { // host: grow instead of dropping - unless at the index limit + m_insertion_failures++; + return false; + } +#else m_insertion_failures++; return false; +#endif } + m_buffer[m_head] = elem; + incrementHead(); + return true; } template @@ -97,6 +109,38 @@ inline void ThreadSafeCircularBuffer::incrementHead() { incrementTail(); } } + +#ifdef RTPS_STORAGE_DYNAMIC +template bool ThreadSafeCircularBuffer::grow() { + // Caller holds m_mutex. Double the ring capacity, then re-linearize the live + // elements so the head/tail indices remain valid after the append-only + // resize. The live range [m_tail, m_head) is move-assigned into the freshly + // grown region [oldCap, ...) in logical order (move-assignment only, so the + // element type need not be move-constructible). Element count is preserved, + // so m_num_elements is left unchanged. + const std::size_t oldCap = m_buffer.size(); + // 16-bit ring indices: refuse to grow past what they can represent, else the + // m_head/m_tail casts below truncate and corrupt the ring. A queue never + // legitimately needs this many slots; the caller then treats it as full. + if (oldCap * 2 > std::numeric_limits::max()) { + return false; + } + m_buffer.ensureSize(oldCap * 2); + std::size_t dst = oldCap; + uint16_t src = m_tail; + while (src != m_head) { + m_buffer[dst] = std::move(m_buffer[src]); + ++dst; + ++src; + if (src >= oldCap) { + src = 0; // wrap at the OLD capacity + } + } + m_tail = static_cast(oldCap); + m_head = static_cast(dst); + return true; +} +#endif } // namespace rtps #endif // RTPS_THREADSAFECIRCULARBUFFER_TPP diff --git a/components/rtps_embedded/include/rtps_participant.hpp b/components/rtps_embedded/include/rtps_participant.hpp index bd47573cd..cd9e4fbc9 100644 --- a/components/rtps_embedded/include/rtps_participant.hpp +++ b/components/rtps_embedded/include/rtps_participant.hpp @@ -72,6 +72,13 @@ class RtpsParticipant : public BaseComponent { 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. + /// Nominal per-fragment payload size (bytes) used when a published sample is + /// too large for a single DATA submessage and is split into DATA_FRAG + /// submessages. Default 63000 (large: fewer fragments). Lower it toward the + /// path MTU (e.g. ~1400) for lossy links. Only relevant when fragmentation is + /// compiled in (always on host; opt-in on ESP32). Ignored for samples that + /// fit a single DATA submessage. + uint16_t fragment_size{63000}; }; /// Configuration for a reader (subscribing endpoint). @@ -130,13 +137,27 @@ class RtpsParticipant : public BaseComponent { /// reader pool is exhausted). bool add_reader(const ReaderConfig &config); - /// Maximum size of a single published CDR payload, in bytes. Bounded by the - /// RTPS wire format: a DATA submessage's length (octetsToNextHeader) and the - /// engine's DataSize_t are both 16-bit, so one unfragmented sample cannot - /// exceed 65535 bytes. Larger payloads (e.g. images) require DATA_FRAG - /// fragmentation, which this engine does not yet implement - publish() rejects - /// oversized samples rather than silently truncating them. - static constexpr std::size_t max_payload_size = 65535; + /// Maximum size of a single published CDR payload, in bytes. + /// + /// When fragmentation is compiled in (RTPS_ENABLE_FRAGMENTATION - always on + /// host, opt-in on ESP32) this is the large-sample reassembly cap + /// (RTPS_MAX_SAMPLE_SIZE: 8 MB host, 256 KB ESP32): samples above one DATA + /// submessage are split into DATA_FRAG submessages and reassembled by the peer. + /// When fragmentation is compiled out (ESP32 default) it is bounded by the RTPS + /// wire format instead: a DATA submessage's length field (octetsToNextHeader) + /// is 16-bit, so one unfragmented sample cannot exceed 65535 bytes, and + /// publish() rejects larger samples rather than truncating them. +#if defined(RTPS_ENABLE_FRAGMENTATION) + static constexpr std::size_t max_payload_size = RTPS_MAX_SAMPLE_SIZE; +#else + // The largest serialized payload that fits one unfragmented DATA submessage in + // a single UDP datagram: 65507 (max UDP payload) - 20 (RTPS header) - 12 + // (INFO_TS) - 24 (DATA submessage header) = 65451. Kept in sync with the + // engine's rtps::MAX_UNFRAGMENTED_PAYLOAD by a static_assert in the .cpp. A + // larger sample would overflow the datagram and wrap the 16-bit submessage + // length, so publish() rejects it. + static constexpr std::size_t max_payload_size = 65451; +#endif /// Publish a CDR-encapsulated sample on a topic previously registered with /// add_writer(). diff --git a/components/rtps_embedded/interop/ros2_big_publisher.py b/components/rtps_embedded/interop/ros2_big_publisher.py new file mode 100644 index 000000000..ee4b99fee --- /dev/null +++ b/components/rtps_embedded/interop/ros2_big_publisher.py @@ -0,0 +1,46 @@ +#!/usr/bin/env python3 +"""Publish a deterministic 200 KB std_msgs/String from a ROS 2 node. + +Used by the interop harness for the ROS 2 -> espp large-payload (DATA_FRAG) leg. +The pattern is generated in-process (not passed on the command line) so it does +not hit the OS ARG_MAX limit that `ros2 topic pub "{data: '<200KB>'}"` does. +Best-effort QoS to match the v1 (best-effort) fragmentation scope. +""" + +import sys +import time + +import rclpy +from rclpy.qos import QoSProfile, ReliabilityPolicy +from std_msgs.msg import String + + +def main() -> int: + topic = sys.argv[1] if len(sys.argv) > 1 else "/bignum2" + size = int(sys.argv[2]) if len(sys.argv) > 2 else 200000 + seconds = float(sys.argv[3]) if len(sys.argv) > 3 else 40.0 + + # Shared pattern the espp subscriber verifies byte-exact: 'A' + (i % 26). + pattern = "".join(chr(65 + (i % 26)) for i in range(size)) + + rclpy.init() + node = rclpy.create_node("bigpub") + qos = QoSProfile(depth=10) + qos.reliability = ReliabilityPolicy.BEST_EFFORT + pub = node.create_publisher(String, topic, qos) + + msg = String() + msg.data = pattern + deadline = time.time() + seconds + while time.time() < deadline: + pub.publish(msg) # FastDDS fragments this 200 KB sample into DATA_FRAG + rclpy.spin_once(node, timeout_sec=0.0) + time.sleep(0.5) + + node.destroy_node() + rclpy.shutdown() + return 0 + + +if __name__ == "__main__": + sys.exit(main()) diff --git a/components/rtps_embedded/interop/run_interop.sh b/components/rtps_embedded/interop/run_interop.sh index 4f1cee4cb..fa4e597f2 100755 --- a/components/rtps_embedded/interop/run_interop.sh +++ b/components/rtps_embedded/interop/run_interop.sh @@ -27,7 +27,8 @@ 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_pubsub rtps_embedded_golden rtps_facade_pubsub rtps_typed_pubsub \ + rtps_facade_frag rtps_facade_backlog rtps_facade_frag_sizes \ rtps_embedded_interop_pub rtps_embedded_interop_sub > /tmp/build.log 2>&1 build_rc=$? result "build" $build_rc @@ -47,6 +48,38 @@ note "espp <-> espp in-process loopback" note "facade <-> facade in-process (two participants, port probing)" "$BIN"/rtps_facade_pubsub; result "facade_loopback" $? +note "typed pub/sub in-process" +"$BIN"/rtps_typed_pubsub; result "typed_loopback" $? + +# Regression guard: a reliable writer under backlog must retain + send every +# sample on the dynamic (host) storage path (no cursor-advance-as-drop skip). +# Non-fragmented small samples, so robust in the shared-netns container. +note "reliable backlog: no sample skipped (dynamic history growth)" +"$BIN"/rtps_facade_backlog; result "backlog_no_skip" $? + +# NOTE: the in-process fragmented loopbacks (rtps_facade_frag, rtps_facade_frag_sizes) +# are BUILT above (compile guard) but run as standalone host gates (docker-free), +# not here: two participants sharing one +# process + reactor in the container, publishing many small MTU-capped fragments, +# is an environment artifact (it passes on the host). The espp<->espp +# fragmentation path is proven in-container by cross_process_frag_200k below. + +note "espp <-> espp cross-process 200 KB DATA_FRAG (reliable, large 60000 frag size, byte-exact)" +# Few large DATA_FRAG submessages (~4 per 200 KB sample), matching FastDDS's +# default large-fragment style; reliable QoS recovers any dropped fragment by +# whole-sample retransmit. Publish slowly so each SN completes (single reassembly +# slot) before the next. +"$BIN"/rtps_embedded_interop_sub xprocfrag std_msgs::msg::dds_::String_ 1 1 40 "" 200000 > /tmp/xsubfrag.log 2>&1 & +XSUBF=$! +sleep 2 +"$BIN"/rtps_embedded_interop_pub xprocfrag std_msgs::msg::dds_::String_ 1 12 3000 "" 200000 60000 > /tmp/xpubfrag.log 2>&1 & +XPUBF=$! +wait $XSUBF +xsubf_rc=$? +kill $XPUBF 2>/dev/null; wait $XPUBF 2>/dev/null +tail -2 /tmp/xsubfrag.log +result "cross_process_frag_200k" $xsubf_rc + note "espp <-> espp cross-process on one host (port probing)" "$BIN"/rtps_embedded_interop_sub xproc std_msgs::msg::dds_::String_ 0 3 20 > /tmp/xsub.log 2>&1 & XSUB=$! @@ -90,6 +123,43 @@ echo2_rc=$? kill $ESPP_PID 2>/dev/null; wait $ESPP_PID 2>/dev/null result "espp_be_pub->ros2_be_echo" $echo2_rc +note "espp best-effort publisher -> ROS 2 (200 KB String, DATA_FRAG)" +# espp fragments the 200 KB sample into DATA_FRAG submessages; rmw_fastrtps +# reassembles it. Best-effort (v1 scope): in the shared-netns container there is +# ~no loss, so this proves the espp->ROS 2 DATA_FRAG WIRE encoding interops. +"$BIN"/rtps_embedded_interop_pub rt/bignum std_msgs::msg::dds_::String_ 0 20 2000 "" 200000 60000 > /tmp/pubbig.log 2>&1 & +ESPP_PID=$! +sleep 3 +timeout 45 ros2 topic echo --once --full-length --qos-reliability best_effort /bignum std_msgs/msg/String > /tmp/echobig.log 2>&1 +echobig_rc=$? +kill $ESPP_PID 2>/dev/null; wait $ESPP_PID 2>/dev/null +echo "echo exit=$echobig_rc, echo bytes=$(wc -c < /tmp/echobig.log)" +echo "--- pubbig tail ---"; tail -3 /tmp/pubbig.log +echo "--- echobig head ---"; head -c 160 /tmp/echobig.log; echo +# Require the echo to succeed AND to have carried a large (fragment-reassembled) +# payload (>150 KB of YAML), proving the 200 KB sample crossed intact. +big1_rc=1 +if [ "$echobig_rc" -eq 0 ] && [ "$(wc -c < /tmp/echobig.log)" -gt 150000 ]; then big1_rc=0; fi +result "espp_pub->ros2_echo_200k" $big1_rc + +note "ROS 2 publisher -> espp subscriber (200 KB String, DATA_FRAG both vendors)" +# ros2/FastDDS fragments the 200 KB String (its own fragment size); espp must +# reassemble it byte-exact against the shared 'A'+(i%26) pattern. Best-effort +# (v1): proves espp decodes FastDDS DATA_FRAG. Reliable frag recovery is v2. +"$BIN"/rtps_embedded_interop_sub rt/bignum2 std_msgs::msg::dds_::String_ 0 1 45 "" 200000 > /tmp/subbig.log 2>&1 & +SUBBIG_PID=$! +sleep 3 +# 200 KB cannot be passed as a `ros2 topic pub` CLI arg (ARG_MAX); publish it +# from a small rclpy node that generates the pattern in-process (best-effort). +timeout 45 python3 /work/components/rtps_embedded/interop/ros2_big_publisher.py /bignum2 200000 40 > /tmp/rospubbig.log 2>&1 & +ROSBIG_PID=$! +wait $SUBBIG_PID +big2_rc=$? +kill $ROSBIG_PID 2>/dev/null; wait $ROSBIG_PID 2>/dev/null +echo "--- subbig tail ---"; tail -20 /tmp/subbig.log +echo "--- rospubbig tail ---"; tail -5 /tmp/rospubbig.log +result "ros2_pub->espp_sub_200k" $big2_rc + echo "" echo "==================== SUMMARY ====================" echo "PASS=$PASS FAIL=$FAIL" diff --git a/components/rtps_embedded/src/communication/EsppTransport.cpp b/components/rtps_embedded/src/communication/EsppTransport.cpp index f270e2819..09e8fac2f 100644 --- a/components/rtps_embedded/src/communication/EsppTransport.cpp +++ b/components/rtps_embedded/src/communication/EsppTransport.cpp @@ -1,7 +1,6 @@ /* The MIT License -Copyright (c) 2019 Lehrstuhl Informatik 11 - RWTH Aachen University -Modifications Copyright (c) 2026 ATDev +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 @@ -18,9 +17,7 @@ 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 +This file is part of the espp embeddedRTPS port. */ #include "rtps/communication/EsppTransport.hpp" @@ -33,6 +30,14 @@ Author: i11 - Embedded Software, RWTH Aachen University #include #include +#ifdef RTPS_ENABLE_FRAGMENTATION +#if defined(_WIN32) +#include +#else +#include +#endif +#endif + using rtps::EsppTransport; namespace { @@ -125,7 +130,16 @@ bool EsppTransport::startReceiver(Channel &channel, Ip4Port_t receivePort) { // preserving RTPS's per-locator ordering. espp::UdpSocket::ReceiveConfig receive_config; receive_config.port = receivePort; +#ifdef RTPS_ENABLE_FRAGMENTATION + // With fragmentation enabled a peer (e.g. FastDDS/ROS 2) may send DATA_FRAG + // fragments as large as a full UDP datagram (~64 KB), so the per-datagram read + // buffer must accept any single datagram or large fragments get truncated on + // receive. The ESP32 default build (fragmentation off) keeps the small 8 KB + // buffer, paying nothing for this. + receive_config.buffer_size = 64 * 1024; +#else receive_config.buffer_size = 1024 * 8; +#endif receive_config.on_receive_callback = [this, receivePort](std::vector &data, const espp::Socket::Info &sender) -> std::optional> { @@ -156,6 +170,18 @@ EsppTransport::Channel *EsppTransport::createChannel(Ip4Port_t receivePort, bool return nullptr; } +#ifdef RTPS_ENABLE_FRAGMENTATION + // Enlarge the kernel receive buffer so a burst of DATA_FRAG datagrams for one + // large (fragmented) sample is not dropped before the reactor drains it. + // Best-effort: some stacks clamp SO_RCVBUF, so failure is ignored. Only + // compiled when fragmentation is enabled (never on the ESP32 default build). + { + int rcvbuf = 4 * 1024 * 1024; // request 4 MB (kernel may clamp) + ::setsockopt(channel.socket->native_handle(), SOL_SOCKET, SO_RCVBUF, + reinterpret_cast(&rcvbuf), sizeof(rcvbuf)); + } +#endif + if (!allow_reuse && !channel.socket->disable_reuse()) { logger_.error("Failed to disable port reuse for unicast port {}", receivePort); channel.socket.reset(); diff --git a/components/rtps_embedded/src/entities/Reader.cpp b/components/rtps_embedded/src/entities/Reader.cpp index f2406955c..5f3858b38 100644 --- a/components/rtps_embedded/src/entities/Reader.cpp +++ b/components/rtps_embedded/src/entities/Reader.cpp @@ -1,3 +1,4 @@ +#include #include #include #include @@ -23,6 +24,99 @@ void Reader::executeCallbacks(const ReaderCacheChange &cacheChange) { bool Reader::initMutex() { return true; } +#ifdef RTPS_ENABLE_FRAGMENTATION +void Reader::newFragment(const Guid_t &writerGuid, const SequenceNumber_t &sn, + uint32_t fragmentStartingNum, uint16_t fragmentsInSubmessage, + uint16_t fragmentSize, uint32_t sampleSize, const uint8_t *fragData, + DataSize_t fragDataLen) { + if (fragmentSize == 0 || sampleSize == 0 || fragmentStartingNum == 0) { + return; + } + if (sampleSize > Config::MAX_SAMPLE_SIZE) { + logger_.warn("Dropping fragmented sample: sampleSize {} exceeds MAX_SAMPLE_SIZE {}", + static_cast(sampleSize), static_cast(Config::MAX_SAMPLE_SIZE)); + return; + } + + std::vector completedBuffer; // moved out on completion; owns the bytes + bool haveCompleted = false; + + { + std::lock_guard lock(m_reassembly_mutex); + + // Start a fresh reassembly if this is a different sample (best-effort + // eviction of any older incomplete one) or nothing is in flight. + const bool sameSample = + m_reassembly.active && m_reassembly.writerGuid == writerGuid && m_reassembly.sn == sn; + if (!sameSample) { + m_reassembly.active = true; + m_reassembly.writerGuid = writerGuid; + m_reassembly.sn = sn; + m_reassembly.sampleSize = sampleSize; + m_reassembly.fragmentSize = fragmentSize; + m_reassembly.totalFragments = (sampleSize + fragmentSize - 1) / fragmentSize; + m_reassembly.receivedFragments = 0; + m_reassembly.buffer.assign(sampleSize, 0); + m_reassembly.received.assign(m_reassembly.totalFragments, false); + } else if (m_reassembly.sampleSize != sampleSize || m_reassembly.fragmentSize != fragmentSize) { + // Inconsistent metadata within one SN: drop and reset. + m_reassembly.active = false; + return; + } + + // Copy the fragment payload at its byte offset. fragmentsInSubmessage packed + // fragments are contiguous, so a single memcpy covers them all. + const uint64_t offset = + static_cast(fragmentStartingNum - 1) * m_reassembly.fragmentSize; + if (offset >= sampleSize) { + return; // out-of-range fragment; ignore + } + // A submessage may pack several contiguous fragments (fragmentsInSubmessage). + // Require the serialized data to actually contain all advertised fragments + // before copying or marking them received - otherwise a short DATA_FRAG could + // mark missing ranges complete and deliver zero-filled bytes. The expected + // length is fragmentSize per fragment, except the sample's final fragment + // ends at sampleSize; fragDataLen may be LARGER (trailing 4-byte alignment + // padding, which FastDDS adds) but must not be smaller. + if (fragmentsInSubmessage == 0) { + return; // must advertise at least one fragment + } + const uint64_t expected = + std::min(static_cast(fragmentsInSubmessage) * m_reassembly.fragmentSize, + sampleSize - offset); + if (fragDataLen < expected) { + return; // short / malformed submessage; do not mark ranges complete + } + if (expected > 0 && fragData != nullptr) { + std::memcpy(m_reassembly.buffer.data() + offset, fragData, expected); + } + for (uint16_t k = 0; k < fragmentsInSubmessage; ++k) { + const uint32_t idx = fragmentStartingNum - 1 + k; + if (idx < m_reassembly.totalFragments && !m_reassembly.received[idx]) { + m_reassembly.received[idx] = true; + ++m_reassembly.receivedFragments; + } + } + + if (m_reassembly.receivedFragments >= m_reassembly.totalFragments) { + // Sample complete: move the assembled buffer out and deliver it AFTER + // releasing the reassembly lock (the callback runs synchronously and must + // not be invoked under this lock). + completedBuffer = std::move(m_reassembly.buffer); + haveCompleted = true; + m_reassembly.active = false; + } + } + + if (haveCompleted) { + Guid_t guid = writerGuid; + ReaderCacheChange completed{ChangeKind_t::ALIVE, guid, sn, completedBuffer.data(), + static_cast(completedBuffer.size())}; + newChange(completed); + } +} +#endif + void Reader::reset() { std::lock_guard lock1(m_proxies_mutex); std::lock_guard lock2(m_callback_mutex); diff --git a/components/rtps_embedded/src/entities/StatefulWriter.cpp b/components/rtps_embedded/src/entities/StatefulWriter.cpp index b77f7cd7f..c39e5a436 100644 --- a/components/rtps_embedded/src/entities/StatefulWriter.cpp +++ b/components/rtps_embedded/src/entities/StatefulWriter.cpp @@ -98,17 +98,28 @@ const rtps::CacheChange *StatefulWriter::newChange(ChangeKind_t kind, const uint 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); - } + // A full history may drop its oldest change on the next addChange: the static + // ring always does, and the dynamic (host) ring does too when it hits its + // 16-bit index ceiling and grow() refuses to resize. When a drop happens the + // send cursor may point at a sequence number that no longer exists, which + // would stall progress() permanently; advance it past the drop. When the + // dynamic ring instead grows and RETAINS the oldest (the common host case) the + // minimum is unchanged, so the cursor stays put and the retained change is + // still sent. Distinguish the two by whether the history minimum advanced + // across the add - a copy, not a reference, because addChange may overwrite + // the underlying slot. + const bool wasFull = m_history.isFull(); + const SequenceNumber_t minBefore = m_history.getCurrentSeqNumMin(); auto *result = m_history.addChange(data, size, inLineQoS, markDisposedAfterWrite); + + if (wasFull) { + const SequenceNumber_t minAfter = m_history.getCurrentSeqNumMin(); + if (minBefore < minAfter && m_nextSequenceNumberToSend < minAfter) { + m_nextSequenceNumberToSend = minAfter; // Skip past the dropped change + SFW_LOG("History full, dropped oldest {}.", this->m_attributes.topicName); + } + } if (m_transport != nullptr) { // Run the send asynchronously on the transport's worker pool (never inline // under the caller's locks), matching the previous ThreadPool semantics. @@ -320,6 +331,13 @@ bool StatefulWriter::sendData(const ReaderProxy &reader, const CacheChange *next info.destAddr = locator.getIp4AddressBytes(); info.destPort = (Ip4Port_t)locator.port; +#ifdef RTPS_ENABLE_FRAGMENTATION + if (next->data.spaceUsed() > MAX_UNFRAGMENTED_PAYLOAD) { + return sendSampleFragmented(info.destAddr, info.destPort, reader.remoteReaderGuid.entityId, + next); + } +#endif + MessageFactory::addSubMessageData(payload, next->data, next->inLineQoS, next->sequenceNumber, m_attributes.endpointGuid.entityId, reader.remoteReaderGuid.entityId); @@ -332,6 +350,44 @@ bool StatefulWriter::sendData(const ReaderProxy &reader, const CacheChange *next return true; } +#ifdef RTPS_ENABLE_FRAGMENTATION +bool StatefulWriter::sendSampleFragmented(const Ip4AddressBytes &destAddr, Ip4Port_t destPort, + const EntityId_t &readerId, const CacheChange *next) { + INIT_GUARD() + const uint8_t *sampleData = next->data.bytes.data(); + const uint32_t sampleSize = next->data.spaceUsed(); + const uint16_t fragmentSize = m_fragmentSize > MAX_FRAGMENT_SIZE + ? static_cast(MAX_FRAGMENT_SIZE) + : m_fragmentSize; + if (fragmentSize == 0) { + return false; + } + const uint32_t numFragments = (sampleSize + fragmentSize - 1) / fragmentSize; + for (uint32_t f = 0; f < numFragments; ++f) { + const uint32_t offset = f * fragmentSize; + const uint16_t fragLen = static_cast( + (sampleSize - offset) < fragmentSize ? (sampleSize - offset) : fragmentSize); + + PacketInfo info; + info.srcPort = m_srcPort; + info.destAddr = destAddr; + info.destPort = destPort; + PayloadBuffer payload; + MessageFactory::addHeader(payload, m_attributes.endpointGuid.prefix); + MessageFactory::addSubMessageTimeStamp(payload); + MessageFactory::addSubMessageDataFrag(payload, sampleData + offset, fragLen, f + 1, 1, + fragmentSize, sampleSize, next->sequenceNumber, + m_attributes.endpointGuid.entityId, readerId); + info.payload = std::move(payload.bytes); + if (info.payload.empty()) { + return false; + } + m_transport->sendPacket(info); + } + return true; +} +#endif + void StatefulWriter::sendGap(const ReaderProxy &reader, const SequenceNumber_t &firstMissing, const SequenceNumber_t &nextValid) { INIT_GUARD() @@ -388,6 +444,12 @@ bool StatefulWriter::sendDataWRMulticast(const ReaderProxy &reader, const CacheC reid = reader.remoteReaderGuid.entityId; } +#ifdef RTPS_ENABLE_FRAGMENTATION + if (next->data.spaceUsed() > MAX_UNFRAGMENTED_PAYLOAD) { + return sendSampleFragmented(info.destAddr, info.destPort, reid, next); + } +#endif + MessageFactory::addSubMessageData(payload, next->data, next->inLineQoS, next->sequenceNumber, m_attributes.endpointGuid.entityId, reid); diff --git a/components/rtps_embedded/src/entities/StatelessWriter.cpp b/components/rtps_embedded/src/entities/StatelessWriter.cpp index 26bfa9bf0..83274a125 100644 --- a/components/rtps_embedded/src/entities/StatelessWriter.cpp +++ b/components/rtps_embedded/src/entities/StatelessWriter.cpp @@ -91,15 +91,28 @@ const CacheChange *StatelessWriter::newChange(rtps::ChangeKind_t kind, const uin 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); - } + // A full history may drop its oldest sample on the next addChange: the static + // ring always does, and the dynamic (host) ring does too when it hits its + // 16-bit index ceiling and grow() refuses to resize. When a drop happens the + // send cursor may point at a sequence number that no longer exists, which + // would stall progress() permanently; advance it past the drop. When the + // dynamic ring instead grows and RETAINS the oldest (the common host case) the + // minimum is unchanged, so the cursor stays put and the retained sample is + // still sent. Distinguish the two by whether the history minimum advanced + // across the add - a copy, not a reference, because addChange may overwrite + // the underlying slot. + const bool wasFull = m_history.isFull(); + const SequenceNumber_t minBefore = m_history.getSeqNumMin(); auto *result = m_history.addChange(data, size); + + if (wasFull) { + const SequenceNumber_t minAfter = m_history.getSeqNumMin(); + if (minBefore < minAfter && m_nextSequenceNumberToSend < minAfter) { + m_nextSequenceNumberToSend = minAfter; // Skip past the dropped sample + SLW_LOG("History full, dropped oldest {}", this->m_attributes.topicName); + } + } if (m_transport != nullptr) { // Run the send asynchronously on the transport's worker pool (never inline // under the caller's locks), matching the previous ThreadPool semantics. @@ -152,6 +165,15 @@ void StatelessWriter::progress() { info.srcPort = m_srcPort; PayloadBuffer payload; + // 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; + } + MessageFactory::addHeader(payload, m_attributes.endpointGuid.prefix); MessageFactory::addSubMessageTimeStamp(payload); @@ -177,23 +199,22 @@ void StatelessWriter::progress() { } else { reid = proxy.remoteReaderGuid.entityId; } + +#ifdef RTPS_ENABLE_FRAGMENTATION + if (next->data.spaceUsed() > MAX_UNFRAGMENTED_PAYLOAD) { + // Oversized sample: emit DATA_FRAG submessages (built + sent under the + // lock so next->data stays valid across fragments) and skip the single + // DATA path for this proxy. + sendSampleFragmented(info.destAddr, info.destPort, reid, next); + continue; + } +#endif 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()) { @@ -206,3 +227,40 @@ void StatelessWriter::progress() { m_history.removeUntilIncl(m_nextSequenceNumberToSend); ++m_nextSequenceNumberToSend; } + +#ifdef RTPS_ENABLE_FRAGMENTATION +bool StatelessWriter::sendSampleFragmented(const Ip4AddressBytes &destAddr, Ip4Port_t destPort, + const EntityId_t &readerId, const CacheChange *next) { + const uint8_t *sampleData = next->data.bytes.data(); + const uint32_t sampleSize = next->data.spaceUsed(); + const uint16_t fragmentSize = m_fragmentSize > MAX_FRAGMENT_SIZE + ? static_cast(MAX_FRAGMENT_SIZE) + : m_fragmentSize; + if (fragmentSize == 0) { + return false; + } + const uint32_t numFragments = (sampleSize + fragmentSize - 1) / fragmentSize; + for (uint32_t f = 0; f < numFragments; ++f) { + const uint32_t offset = f * fragmentSize; + const uint16_t fragLen = static_cast( + (sampleSize - offset) < fragmentSize ? (sampleSize - offset) : fragmentSize); + + PacketInfo info; + info.srcPort = m_srcPort; + info.destAddr = destAddr; + info.destPort = destPort; + PayloadBuffer payload; + MessageFactory::addHeader(payload, m_attributes.endpointGuid.prefix); + MessageFactory::addSubMessageTimeStamp(payload); + MessageFactory::addSubMessageDataFrag(payload, sampleData + offset, fragLen, f + 1, 1, + fragmentSize, sampleSize, next->sequenceNumber, + m_attributes.endpointGuid.entityId, readerId); + info.payload = std::move(payload.bytes); + if (info.payload.empty()) { + return false; + } + m_transport->sendPacket(info); + } + return true; +} +#endif diff --git a/components/rtps_embedded/src/messages/MessageReceiver.cpp b/components/rtps_embedded/src/messages/MessageReceiver.cpp index 46d11ad2d..42979eef3 100644 --- a/components/rtps_embedded/src/messages/MessageReceiver.cpp +++ b/components/rtps_embedded/src/messages/MessageReceiver.cpp @@ -105,6 +105,12 @@ bool MessageReceiver::processSubmessage(MessageProcessingInfo &msgInfo, RECV_LOG("Processing Data submessage"); success = processDataSubmessage(msgInfo, submsgHeader, sourceState); break; +#ifdef RTPS_ENABLE_FRAGMENTATION + case SubmessageKind::DATA_FRAG: + RECV_LOG("Processing DataFrag submessage"); + success = processDataFragSubmessage(msgInfo, submsgHeader, sourceState); + break; +#endif case SubmessageKind::HEARTBEAT: RECV_LOG("Processing Heartbeat submessage"); success = processHeartbeatSubmessage(msgInfo, sourceState); @@ -228,6 +234,81 @@ bool MessageReceiver::processDataSubmessage(MessageProcessingInfo &msgInfo, return true; } +#ifdef RTPS_ENABLE_FRAGMENTATION +bool MessageReceiver::processDataFragSubmessage(MessageProcessingInfo &msgInfo, + const SubmessageHeader &submsgHeader, + const rtps::MessageSourceState &sourceState) { + SubmessageDataFrag frag; + if (!deserializeMessage(msgInfo, frag)) { + return false; + } + + const uint8_t *submessageStart = msgInfo.getPointerToCurrentPos(); + const uint8_t *submessageEnd = + submessageStart + SubmessageHeader::getRawSize() + submsgHeader.octetsToNextHeader; + const uint16_t submessageBodyOffset = static_cast( + sizeof(frag.extraFlags) + sizeof(frag.octetsToInlineQos) + frag.octetsToInlineQos); + const uint8_t *serializedData = + submessageStart + SubmessageHeader::getRawSize() + submessageBodyOffset; + + if (serializedData > submessageEnd) { + return false; + } + + // Skip an inlineQos ParameterList if present (a peer may attach a key hash to + // the first fragment), mirroring processDataSubmessage. + 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; + } + } + + if (serializedData > submessageEnd) { + return false; + } + const DataSize_t fragDataLen = static_cast(submessageEnd - serializedData); + + Reader *reader; + if (frag.readerId == ENTITYID_UNKNOWN) { + reader = mp_part->getReaderByWriterId(Guid_t{sourceState.sourceGuidPrefix, frag.writerId}); + } else { + reader = mp_part->getReader(frag.readerId); + } + if (reader != nullptr) { + Guid_t writerGuid{sourceState.sourceGuidPrefix, frag.writerId}; + reader->newFragment(writerGuid, frag.writerSN, frag.fragmentStartingNum, + frag.fragmentsInSubmessage, frag.fragmentSize, frag.sampleSize, + serializedData, fragDataLen); + } + return true; +} +#endif + bool MessageReceiver::processHeartbeatSubmessage(MessageProcessingInfo &msgInfo, const rtps::MessageSourceState &sourceState) { SubmessageHeartbeat submsgHB; diff --git a/components/rtps_embedded/src/messages/MessageTypes.cpp b/components/rtps_embedded/src/messages/MessageTypes.cpp index 8ed25a2df..d33ef2811 100644 --- a/components/rtps_embedded/src/messages/MessageTypes.cpp +++ b/components/rtps_embedded/src/messages/MessageTypes.cpp @@ -96,6 +96,48 @@ bool rtps::deserializeMessage(const MessageProcessingInfo &info, SubmessageData return true; } +#ifdef RTPS_ENABLE_FRAGMENTATION +bool rtps::deserializeMessage(const MessageProcessingInfo &info, SubmessageDataFrag &msg) { + if (info.getRemainingSize() < SubmessageHeader::getRawSize()) { + return false; + } + if (!deserializeMessage(info, msg.header)) { + return false; + } + + // The fixed DATA_FRAG header (without inlineQos / serializedData) must be + // present in full. + constexpr auto min_body_size = SubmessageDataFrag::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)); + doCopyAndMoveOn(reinterpret_cast(&msg.fragmentStartingNum), currentPos, + sizeof(uint32_t)); + doCopyAndMoveOn(reinterpret_cast(&msg.fragmentsInSubmessage), currentPos, + sizeof(uint16_t)); + doCopyAndMoveOn(reinterpret_cast(&msg.fragmentSize), currentPos, sizeof(uint16_t)); + doCopyAndMoveOn(reinterpret_cast(&msg.sampleSize), currentPos, sizeof(uint32_t)); + return true; +} +#endif + bool rtps::deserializeMessage(const MessageProcessingInfo &info, SubmessageHeartbeat &msg) { if (info.getRemainingSize() < SubmessageHeartbeat::getRawSize()) { return false; diff --git a/components/rtps_embedded/src/rtps_participant.cpp b/components/rtps_embedded/src/rtps_participant.cpp index 930fedf2c..ebdaa21c3 100644 --- a/components/rtps_embedded/src/rtps_participant.cpp +++ b/components/rtps_embedded/src/rtps_participant.cpp @@ -190,6 +190,13 @@ bool RtpsParticipant::add_writer(const WriterConfig &config) { logger_.error("Writer for topic '{}' already exists", config.topic); return false; } + // A zero fragment size is invalid: the fragmented send paths treat it as + // failure and would silently drop every large sample while publish() reports + // success. Reject it up front rather than creating a broken writer. + if (config.fragment_size == 0) { + logger_.error("Writer '{}': fragment_size must be non-zero", config.topic); + return false; + } rtps::Writer *writer = domain_->createWriter(*participant_, config.topic.c_str(), config.type_name.c_str(), config.reliability == Reliability::RELIABLE); @@ -198,6 +205,9 @@ bool RtpsParticipant::add_writer(const WriterConfig &config) { config.topic); return false; } + // Per-writer fragment size (only used when a sample exceeds a single DATA + // submessage and fragmentation is compiled in; otherwise inert). + writer->setFragmentSize(config.fragment_size); writers_[config.topic] = writer; logger_.info("Added {} writer: topic='{}' type='{}'", config.reliability == Reliability::RELIABLE ? "reliable" : "best-effort", @@ -246,16 +256,29 @@ bool RtpsParticipant::publish(std::string_view topic, std::span c logger_.error("No writer for topic '{}'", topic); return false; } - // Reject rather than silently truncate: DataSize_t (and the RTPS submessage - // length field) is 16-bit, so casting a larger size would wrap. Keep the - // header constant in sync with the engine's actual DataSize_t bound. - static_assert(RtpsParticipant::max_payload_size == std::numeric_limits::max()); + // Reject rather than silently truncate. +#if defined(RTPS_ENABLE_FRAGMENTATION) + // With fragmentation compiled in, the cap is the large-sample reassembly bound + // (RTPS_MAX_SAMPLE_SIZE). Samples above one DATA submessage are split into + // DATA_FRAG submessages by the engine writer. + if (cdr_payload.size() > max_payload_size) { + logger_.error( + "Payload for topic '{}' is {} bytes, exceeds the {}-byte max sample size; dropped", topic, + cdr_payload.size(), max_payload_size); + return false; + } +#else + // Without fragmentation, the binding cap is the RTPS wire format: a single DATA + // submessage must fit one UDP datagram and its 16-bit octetsToNextHeader. Keep + // the facade constant in sync with the engine's actual computed bound. + static_assert(RtpsParticipant::max_payload_size == rtps::MAX_UNFRAGMENTED_PAYLOAD); if (cdr_payload.size() > max_payload_size) { logger_.error("Payload for topic '{}' is {} bytes, exceeds the {}-byte RTPS limit; dropped " - "(large payloads need DATA_FRAG fragmentation, not yet supported)", + "(large payloads need DATA_FRAG fragmentation, not enabled in this build)", topic, cdr_payload.size(), max_payload_size); return false; } +#endif const auto *change = it->second->newChange(rtps::ChangeKind_t::ALIVE, cdr_payload.data(), static_cast(cdr_payload.size())); if (change == nullptr) { diff --git a/lib/espp.cmake b/lib/espp.cmake index 3e4c1f73c..5f805e2a8 100644 --- a/lib/espp.cmake +++ b/lib/espp.cmake @@ -1,5 +1,58 @@ set(ESPP_COMPONENTS "${CMAKE_CURRENT_LIST_DIR}/../components") +# --------------------------------------------------------------------------- +# RTPS static-limits profile selection. +# +# The rtps_embedded engine keeps a fully-static (deterministic) allocation model +# whose compile-time capacity caps are chosen by a profile header (see +# components/rtps_embedded/include/rtps/config.hpp). Profiles: +# embedded - tight MCU caps (rtps/config_esp32.hpp) +# host - relaxed static caps, DEFAULT for non-ESP builds +# (rtps/config_desktop.hpp) +# host_large - generous static caps for large DDS graphs +# (rtps/config_host_large.hpp) +# +# Select with -DRTPS_LIMITS_PROFILE=embedded|host|host_large. Default is "host" +# (this file is only used for non-ESP / host builds; ESP/IDF builds pick the +# profile via Kconfig in components/rtps_embedded/Kconfig). +# --------------------------------------------------------------------------- +if(NOT DEFINED RTPS_LIMITS_PROFILE) + set(RTPS_LIMITS_PROFILE "host") +endif() +set(RTPS_LIMITS_PROFILE "${RTPS_LIMITS_PROFILE}" CACHE STRING + "RTPS static limits profile: embedded|host|host_large") +set_property(CACHE RTPS_LIMITS_PROFILE PROPERTY STRINGS embedded host host_large) + +if(RTPS_LIMITS_PROFILE STREQUAL "embedded") + add_compile_definitions(RTPS_CONFIG_HEADER="rtps/config_esp32.hpp") + set(RTPS_MAX_SAMPLE_SIZE 262144) # 256 KB (matches config_esp32.hpp) +elseif(RTPS_LIMITS_PROFILE STREQUAL "host") + add_compile_definitions(RTPS_CONFIG_HEADER="rtps/config_desktop.hpp") + set(RTPS_MAX_SAMPLE_SIZE 8388608) # 8 MB (matches config_desktop.hpp) +elseif(RTPS_LIMITS_PROFILE STREQUAL "host_large") + add_compile_definitions(RTPS_CONFIG_HEADER="rtps/config_host_large.hpp") + set(RTPS_MAX_SAMPLE_SIZE 8388608) # 8 MB (matches config_host_large.hpp) +else() + message(FATAL_ERROR + "Invalid RTPS_LIMITS_PROFILE '${RTPS_LIMITS_PROFILE}' " + "(expected: embedded | host | host_large)") +endif() +message(STATUS "RTPS limits profile: ${RTPS_LIMITS_PROFILE}") + +# --------------------------------------------------------------------------- +# RTPS best-effort DATA_FRAG fragmentation (Slice C). +# +# On host builds fragmentation is ALWAYS enabled: samples larger than a single +# DATA submessage are split into DATA_FRAG submessages and reassembled by the +# peer (interoperates with FastDDS / ROS 2). RTPS_MAX_SAMPLE_SIZE is the +# large-sample reassembly cap and is kept in sync with the profile's +# Config::MAX_SAMPLE_SIZE. On ESP32/IDF fragmentation is opt-in via Kconfig (see +# components/rtps_embedded/Kconfig / CMakeLists.txt), default off, so the MCU +# pays nothing for it by default. +# --------------------------------------------------------------------------- +add_compile_definitions(RTPS_ENABLE_FRAGMENTATION RTPS_MAX_SAMPLE_SIZE=${RTPS_MAX_SAMPLE_SIZE}) +message(STATUS "RTPS fragmentation: ON (max sample size ${RTPS_MAX_SAMPLE_SIZE} bytes)") + set(ESPP_EXTERNAL_INCLUDES ${ESPP_COMPONENTS}/serialization/detail/alpaca/include ${ESPP_COMPONENTS}/cli/detail/cli/include diff --git a/pc/tests/rtps_embedded_golden.cpp b/pc/tests/rtps_embedded_golden.cpp index 637428748..030b8e38b 100644 --- a/pc/tests/rtps_embedded_golden.cpp +++ b/pc/tests/rtps_embedded_golden.cpp @@ -69,6 +69,20 @@ std::vector build_data() { return b.bytes; } +std::vector build_data_frag() { + // One DATA_FRAG submessage: fragment #1 of a 200000-byte sample split at a + // 63000-byte fragment size, carrying a 10-byte ramp payload chunk. Pins the + // DATA_FRAG wire layout (extraFlags, octetsToInlineQos, ids, writerSN, + // fragmentStartingNum, fragmentsInSubmessage, fragmentSize, sampleSize, data). + static constexpr uint8_t kFrag[] = {0x00, 0x01, 0x02, 0x03, 0x04, 0x05, 0x06, 0x07, 0x08, 0x09}; + rtps::PayloadBuffer b; + rtps::MessageFactory::addSubMessageDataFrag(b, kFrag, sizeof(kFrag), /*fragStartNum=*/1, + /*fragsInSubmsg=*/1, /*fragmentSize=*/63000, + /*sampleSize=*/200000, 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}, @@ -191,6 +205,7 @@ int main(int argc, char **argv) { {"info_dst", build_info_dst, kGolden_info_dst}, {"info_ts_invalid", build_info_ts_invalid, kGolden_info_ts_invalid}, {"data", build_data, kGolden_data}, + {"data_frag", build_data_frag, kGolden_data_frag}, {"heartbeat", build_heartbeat, kGolden_heartbeat}, {"acknack", build_acknack, kGolden_acknack}, {"gap", build_gap, kGolden_gap}, diff --git a/pc/tests/rtps_embedded_golden.inc b/pc/tests/rtps_embedded_golden.inc index 39ed0c84e..12eb92aca 100644 --- a/pc/tests/rtps_embedded_golden.inc +++ b/pc/tests/rtps_embedded_golden.inc @@ -14,6 +14,11 @@ static constexpr uint8_t kGolden_data[] = { 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_data_frag[] = { + 0x16, 0x01, 0x2A, 0x00, 0x00, 0x00, 0x1C, 0x00, 0x00, 0x00, 0x02, 0x04, + 0x00, 0x00, 0x01, 0x03, 0x00, 0x00, 0x00, 0x00, 0x05, 0x00, 0x00, 0x00, + 0x01, 0x00, 0x00, 0x00, 0x01, 0x00, 0x18, 0xF6, 0x40, 0x0D, 0x03, 0x00, + 0x00, 0x01, 0x02, 0x03, 0x04, 0x05, 0x06, 0x07, 0x08, 0x09,}; 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, diff --git a/pc/tests/rtps_embedded_interop_pub.cpp b/pc/tests/rtps_embedded_interop_pub.cpp index 7f8d5477c..109e6faf8 100644 --- a/pc/tests/rtps_embedded_interop_pub.cpp +++ b/pc/tests/rtps_embedded_interop_pub.cpp @@ -6,7 +6,11 @@ // 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. +// [interface_ip] [payload_bytes] +// When payload_bytes > 0, publishes a String whose data is a deterministic +// payload_bytes-long ASCII pattern (>64 KB exercises DATA_FRAG fragmentation); +// otherwise publishes a short "espp interop N" string. Exits 0 after publishing +// `count` samples. #include #include @@ -24,6 +28,17 @@ struct StringMsg { std::string data; }; +// Deterministic printable pattern shared by the pub, the sub, and the ROS 2 +// generator in run_interop.sh, so large-payload reassembly can be checked +// byte-exact in both directions. +inline std::string make_pattern(std::size_t n) { + std::string s(n, '\0'); + for (std::size_t i = 0; i < n; ++i) { + s[i] = static_cast('A' + (i % 26)); + } + return s; +} + // 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) { @@ -39,6 +54,11 @@ int main(int argc, char **argv) { 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 + const std::size_t payload_bytes = (argc > 7) ? std::strtoul(argv[7], nullptr, 10) : 0; + // Optional per-writer fragment size (bytes). 0 -> keep the facade default + // (63000). Useful on hosts whose UDP max datagram is small (e.g. macOS caps + // net.inet.udp.maxdgram at 9216) so fragments still fit one datagram. + const int fragment_size = (argc > 8) ? std::atoi(argv[8]) : 0; espp::RtpsParticipant participant({ .interface_address = interface_ip, @@ -49,23 +69,32 @@ int main(int argc, char **argv) { return 1; } using Reliability = espp::RtpsParticipant::Reliability; - if (!participant.add_writer({ - .topic = topic, - .type_name = type, - .reliability = reliable ? Reliability::RELIABLE : Reliability::BEST_EFFORT, - })) { + espp::RtpsParticipant::WriterConfig wcfg{ + .topic = topic, + .type_name = type, + .reliability = reliable ? Reliability::RELIABLE : Reliability::BEST_EFFORT, + }; + if (fragment_size > 0) { + wcfg.fragment_size = static_cast(fragment_size); + } + if (!participant.add_writer(wcfg)) { 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); + std::printf("interop_pub: topic=%s type=%s reliable=%d count=%d payload_bytes=%zu\n", topic, type, + reliable ? 1 : 0, count, payload_bytes); + + // Precompute the large deterministic pattern once (if in large-payload mode). + const std::string pattern = payload_bytes > 0 ? make_pattern(payload_bytes) : std::string{}; // 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)}); + auto bytes = payload_bytes > 0 + ? cdr::serialize(StringMsg{pattern}) + : cdr::serialize(StringMsg{"espp interop " + std::to_string(i)}); if (bytes && participant.publish(topic, u8_span(*bytes))) { sent++; std::printf("sent %d\n", sent); diff --git a/pc/tests/rtps_embedded_interop_sub.cpp b/pc/tests/rtps_embedded_interop_sub.cpp index fd284c9b8..f9482887a 100644 --- a/pc/tests/rtps_embedded_interop_sub.cpp +++ b/pc/tests/rtps_embedded_interop_sub.cpp @@ -6,7 +6,11 @@ // 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`. +// [interface_ip] [payload_bytes] +// When payload_bytes > 0, each received String is verified byte-exact against the +// deterministic payload_bytes-long pattern (proving fragmented >64 KB samples are +// reassembled correctly); only byte-exact receptions count toward `required`. +// Exits 0 once `required` samples arrive within `timeout_s`. #include #include @@ -25,6 +29,16 @@ struct StringMsg { std::string data; }; +// Deterministic printable pattern shared with rtps_embedded_interop_pub and the +// ROS 2 generator in run_interop.sh (see that file). +inline std::string make_pattern(std::size_t n) { + std::string s(n, '\0'); + for (std::size_t i = 0; i < n; ++i) { + s[i] = static_cast('A' + (i % 26)); + } + return s; +} + // 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) { @@ -40,6 +54,8 @@ int main(int argc, char **argv) { 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 + const std::size_t payload_bytes = (argc > 7) ? std::strtoul(argv[7], nullptr, 10) : 0; + const std::string expected = payload_bytes > 0 ? make_pattern(payload_bytes) : std::string{}; std::atomic received{0}; @@ -57,9 +73,24 @@ int main(int argc, char **argv) { .type_name = type, .reliability = reliable ? Reliability::RELIABLE : Reliability::BEST_EFFORT, .on_sample = - [&received](std::span cdr_payload) { + [&received, payload_bytes, &expected](std::span cdr_payload) { auto msg = cdr::deserialize(std::as_bytes(cdr_payload)); - if (msg) { + if (!msg) { + return; + } + if (payload_bytes > 0) { + // Large-payload mode: only a byte-exact reassembly counts. + const bool exact = msg->data.size() == payload_bytes && msg->data == expected; + if (!exact) { + std::printf("received %zu bytes, byte-exact=0 (expected %zu)\n", + msg->data.size(), payload_bytes); + std::fflush(stdout); + return; + } + const int n = received.fetch_add(1) + 1; + std::printf("received %d: %zu bytes byte-exact\n", n, msg->data.size()); + std::fflush(stdout); + } else { const int n = received.fetch_add(1) + 1; std::printf("received %d: '%s'\n", n, msg->data.c_str()); std::fflush(stdout); diff --git a/pc/tests/rtps_facade_backlog.cpp b/pc/tests/rtps_facade_backlog.cpp new file mode 100644 index 000000000..dc12daa35 --- /dev/null +++ b/pc/tests/rtps_facade_backlog.cpp @@ -0,0 +1,140 @@ +// Regression tests for two Phase 4 hazards that the plain loopback tests do NOT +// exercise (they never backlog the writer history): +// +// 1. No sample is skipped under backlog. A reliable writer that fills its +// history must, on the dynamic (host) storage path, GROW and retain every +// sample rather than dropping the oldest - and it must NOT advance its send +// cursor as if a drop occurred. Regression guard for the +// StatelessWriter/StatefulWriter cursor-advance vs dynamic-growth bug: the +// writers used to advance m_nextSequenceNumberToSend on isFull(), skipping +// the retained oldest samples so they were never sent. +// +// 2. fragment_size == 0 is rejected by add_writer (a zero fragment size makes +// the fragmented send path fail while publish() would otherwise report +// success and silently drop every large sample). +// +// Two participants in one process, RELIABLE QoS. Exits 0 iff both checks pass. + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "rtps_participant.hpp" + +using namespace std::chrono_literals; + +namespace { +using Reliability = espp::RtpsParticipant::Reliability; + +// 8-byte payload: 4-byte CDR_LE encapsulation header + a little-endian uint32 +// sequence value. The engine transports the bytes verbatim; we only need to +// recover the sequence number on the receive side. +std::vector encode_seq(uint32_t seq) { + std::vector b{0x00, 0x01, 0x00, 0x00, 0, 0, 0, 0}; + b[4] = static_cast(seq & 0xFF); + b[5] = static_cast((seq >> 8) & 0xFF); + b[6] = static_cast((seq >> 16) & 0xFF); + b[7] = static_cast((seq >> 24) & 0xFF); + return b; +} +uint32_t decode_seq(std::span p) { + return static_cast(p[4]) | (static_cast(p[5]) << 8) | + (static_cast(p[6]) << 16) | (static_cast(p[7]) << 24); +} +} // namespace + +int main() { + const char *type = "std_msgs::msg::dds_::UInt32_"; + + espp::RtpsParticipant pub({.log_level = espp::Logger::Verbosity::WARN}); + espp::RtpsParticipant sub({.log_level = espp::Logger::Verbosity::WARN}); + if (!pub.start() || !sub.start()) { + std::printf("FAIL: start\n"); + return 1; + } + + // --- Check 2: fragment_size == 0 must be rejected. --- + if (pub.add_writer({.topic = "cfg_zero_frag", .type_name = type, .fragment_size = 0})) { + std::printf("FAIL: add_writer accepted fragment_size == 0\n"); + return 1; + } + + // --- Check 1: no sample skipped under backlog. --- + constexpr int N = 300; // >> host history depth, so the history backlogs + grows + const char *topic = "backlog"; + std::mutex m; + std::set got; + + 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 = [&](std::span payload) { + if (payload.size() < 8) { + return; + } + std::lock_guard lk(m); + got.insert(decode_seq(payload)); + }})) { + std::printf("FAIL: add_reader\n"); + return 1; + } + + std::this_thread::sleep_for(2s); // let SEDP match + + // Publish N sequence values fast enough that the reliable writer's history + // fills before delivery drains it - forcing the grow/backlog path. + for (int i = 0; i < N; ++i) { + pub.publish(topic, encode_seq(static_cast(i))); + std::this_thread::sleep_for(2ms); + } + + // Wait for reliable delivery to drain. + const auto deadline = std::chrono::steady_clock::now() + 30s; + for (;;) { + { + std::lock_guard lk(m); + if (static_cast(got.size()) >= N) { + break; + } + } + if (std::chrono::steady_clock::now() >= deadline) { + break; + } + std::this_thread::sleep_for(50ms); + } + + // Verify EVERY sequence 0..N-1 arrived (a skipped/never-sent sample shows up as + // a gap - the exact symptom of the cursor-advance bug). + std::size_t count; + std::vector missing; + { + std::lock_guard lk(m); + count = got.size(); + for (uint32_t i = 0; i < static_cast(N); ++i) { + if (got.find(i) == got.end()) { + missing.push_back(i); + } + } + } + pub.stop(); + sub.stop(); + + std::printf("published=%d received=%zu missing=%zu\n", N, count, missing.size()); + if (missing.empty()) { + std::printf("PASS\n"); + return 0; + } + std::printf("FAIL: %zu samples never delivered (first missing seq=%u)\n", missing.size(), + missing.front()); + return 1; +} diff --git a/pc/tests/rtps_facade_frag.cpp b/pc/tests/rtps_facade_frag.cpp new file mode 100644 index 000000000..d4bd9fe19 --- /dev/null +++ b/pc/tests/rtps_facade_frag.cpp @@ -0,0 +1,119 @@ +// In-process large-sample (DATA_FRAG) loopback: two espp::RtpsParticipant +// instances in one process exchange a >64 KB sample that must be fragmented on +// send and reassembled on receive, and the received bytes are verified to be +// byte-exact against the deterministic pattern that was published. +// +// This is the docker-free proof of Slice C's send-split + reassembly path +// (components/rtps_embedded/REFACTOR_PLAN.md). The engine transports the raw +// payload bytes unchanged, so we publish an arbitrary 200 KB byte ramp (well +// above the ~64 KB single-DATA limit) and assert the subscriber receives the +// identical bytes. +// +// Exits 0 when a byte-exact 200 KB sample is received within the deadline. + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "rtps_participant.hpp" + +using namespace std::chrono_literals; + +namespace { +constexpr std::size_t kPayloadBytes = 200000; // > 64 KB -> must fragment + +std::vector make_pattern(std::size_t n) { + std::vector v(n); + for (std::size_t i = 0; i < n; ++i) { + v[i] = static_cast((i * 131u + 7u) & 0xFFu); + } + return v; +} +} // namespace + +int main() { + constexpr auto kDeadline = 40s; + const char *topic = "frag_loopback"; + const char *type = "std_msgs::msg::dds_::ByteMultiArray_"; + const std::vector pattern = make_pattern(kPayloadBytes); + + std::atomic ok{false}; + std::atomic received{0}; + using Reliability = espp::RtpsParticipant::Reliability; + + espp::RtpsParticipant pub({.log_level = espp::Logger::Verbosity::WARN}); + espp::RtpsParticipant sub({.log_level = espp::Logger::Verbosity::WARN}); + + if (!pub.start() || !sub.start()) { + std::printf("FAIL: start\n"); + return 1; + } + // Fragment size 8000: fits a single UDP datagram on any host (macOS caps + // net.inet.udp.maxdgram at 9216), splitting the 200 KB sample into ~25 + // fragments. RELIABLE QoS + publishing a single sequence number (below) makes + // delivery robust to any fragment loss: a lost fragment leaves the SN + // unacknowledged, the writer re-heartbeats, the reader NACKs, and the whole SN + // is retransmitted; the reader's single reassembly slot accumulates fragments + // across retransmit rounds until complete. The receive side accepts ANY + // peer-chosen fragment size. + if (!pub.add_writer({.topic = topic, + .type_name = type, + .reliability = Reliability::RELIABLE, + .fragment_size = 8000})) { + std::printf("FAIL: add_writer\n"); + return 1; + } + if (!sub.add_reader({.topic = topic, + .type_name = type, + .reliability = Reliability::RELIABLE, + .on_sample = [&](std::span payload) { + received.fetch_add(1); + const bool match = + payload.size() == kPayloadBytes && + std::equal(payload.begin(), payload.end(), pattern.begin()); + if (match) { + ok.store(true); + } else { + // In this branch match is false by construction. + std::printf("received %zu bytes, byte-exact=0\n", payload.size()); + } + }})) { + std::printf("FAIL: add_reader\n"); + return 1; + } + + // Give discovery a moment to match before the (large) sample. + std::this_thread::sleep_for(3s); + + // Publish exactly ONE large sample (a single sequence number) and let RELIABLE + // delivery drive it to completion: any dropped fragment is recovered by the + // writer retransmitting the whole SN on NACK, and the reader's single + // reassembly slot accumulates fragments across retransmit rounds without being + // evicted by a newer SN. Re-publish only as a slow fallback if nothing arrived. + int sent = 0; + const auto start = std::chrono::steady_clock::now(); + while (!ok.load() && std::chrono::steady_clock::now() - start < kDeadline) { + if (pub.publish(topic, std::span(pattern.data(), pattern.size()))) { + sent++; + } + for (int i = 0; i < 120 && !ok.load(); ++i) { // wait up to ~12 s before re-publishing + std::this_thread::sleep_for(100ms); + } + } + + std::printf("sent=%d received=%d byte_exact=%d\n", sent, received.load(), ok.load() ? 1 : 0); + pub.stop(); + sub.stop(); + if (ok.load()) { + std::printf("PASS\n"); + return 0; + } + std::printf("FAIL\n"); + return 1; +} diff --git a/pc/tests/rtps_facade_frag_sizes.cpp b/pc/tests/rtps_facade_frag_sizes.cpp new file mode 100644 index 000000000..237f755c5 --- /dev/null +++ b/pc/tests/rtps_facade_frag_sizes.cpp @@ -0,0 +1,104 @@ +// Regression test for DATA_FRAG reassembly with a NON-exact final fragment. +// +// The existing rtps_facade_frag test uses 200000 bytes at fragment_size 8000, +// which divides evenly (25 x 8000) - so its last fragment is full and the +// short/padded-last-fragment reassembly path is never exercised locally (only +// the docker ROS 2 leg hits it). Here we deliberately pick payload sizes that do +// NOT divide evenly by the fragment size, so the final fragment is short. This +// guards the reader reassembly's fragment-length handling (Reader::newFragment) +// against regressions - the clamp/validate logic that makes FastDDS/ROS 2 +// large-sample interop work. +// +// Both cases use fragment_size 8000: on macOS a UDP datagram is capped at 9216 +// bytes (net.inet.udp.maxdgram), so a single fragment must stay under that; the +// large default 63000 is a LAN/Linux setting. The payload sizes are chosen so +// the sample still exceeds the single-DATA limit (~65451, forcing fragmentation) +// and does NOT divide evenly by the fragment size (forcing a short final +// fragment): +// - 70003 B @ 8000 -> 9 fragments (8x8000 + 6003), short last. +// - 205003 B @ 8000 -> 26 fragments (25x8000 + 5003), short last. +// +// Exits 0 iff every case is received byte-exact. + +#include +#include +#include +#include +#include +#include +#include +#include + +#include "rtps_participant.hpp" + +using namespace std::chrono_literals; + +namespace { +using Reliability = espp::RtpsParticipant::Reliability; + +std::vector make_pattern(std::size_t n) { + std::vector v(n); + for (std::size_t i = 0; i < n; ++i) { + v[i] = static_cast((i * 131u + 7u) & 0xFFu); + } + return v; +} + +// Run one payload-size / fragment-size case through an in-process reliable +// loopback and return true iff the sample is received byte-exact. +bool run_case(std::size_t payload_bytes, uint16_t fragment_size, const char *topic) { + const std::vector pattern = make_pattern(payload_bytes); + std::atomic ok{false}; + const char *type = "std_msgs::msg::dds_::ByteMultiArray_"; + + espp::RtpsParticipant pub({.log_level = espp::Logger::Verbosity::WARN}); + espp::RtpsParticipant sub({.log_level = espp::Logger::Verbosity::WARN}); + if (!pub.start() || !sub.start()) { + return false; + } + if (!pub.add_writer({.topic = topic, + .type_name = type, + .reliability = Reliability::RELIABLE, + .fragment_size = fragment_size})) { + return false; + } + if (!sub.add_reader({.topic = topic, + .type_name = type, + .reliability = Reliability::RELIABLE, + .on_sample = [&](std::span payload) { + if (payload.size() == pattern.size() && + std::equal(payload.begin(), payload.end(), pattern.begin())) { + ok.store(true); + } + }})) { + return false; + } + + std::this_thread::sleep_for(2s); // discovery + const auto deadline = std::chrono::steady_clock::now() + 30s; + while (!ok.load() && std::chrono::steady_clock::now() < deadline) { + pub.publish(topic, std::span(pattern.data(), pattern.size())); + for (int i = 0; i < 100 && !ok.load(); ++i) { + std::this_thread::sleep_for(100ms); + } + } + const bool result = ok.load(); + pub.stop(); + sub.stop(); + std::printf("case payload=%zu frag=%u byte_exact=%d\n", payload_bytes, fragment_size, + result ? 1 : 0); + return result; +} +} // namespace + +int main() { + bool ok = true; + ok &= run_case(70003, 8000, "frag_sizes_a"); // 9 fragments, short last + ok &= run_case(205003, 8000, "frag_sizes_b"); // 26 fragments, short last + if (ok) { + std::printf("PASS\n"); + return 0; + } + std::printf("FAIL\n"); + return 1; +}