diff --git a/.gitignore b/.gitignore index e4c7f6946..ab5c012df 100644 --- a/.gitignore +++ b/.gitignore @@ -34,6 +34,9 @@ # build folder for ESP-IDF build/ +# submodule configuration +components/rtps_embedded/thirdparty/Micro-CDR/include/ucdr/config.h + # we only version sdkconfig.defaults sdkconfig sdkconfig.old diff --git a/.gitmodules b/.gitmodules index 7817faf37..d08d15741 100644 --- a/.gitmodules +++ b/.gitmodules @@ -37,3 +37,6 @@ [submodule "components/gfps_service/detail/nearby"] path = components/gfps_service/detail/nearby url = https://github.com/esp-cpp/nearby +[submodule "components/rtps_embedded/thirdparty/Micro-CDR"] + path = components/rtps_embedded/thirdparty/Micro-CDR + url = git@github.com:esp-cpp/Micro-CDR.git diff --git a/components/rtps_embedded/CMakeLists.txt b/components/rtps_embedded/CMakeLists.txt new file mode 100644 index 000000000..4d13bd42f --- /dev/null +++ b/components/rtps_embedded/CMakeLists.txt @@ -0,0 +1,30 @@ +idf_component_register( + SRCS + "src/ThreadPool.cpp" + "src/communication/EsppTransport.cpp" + "src/discovery/ParticipantProxyData.cpp" + "src/discovery/SEDPAgent.cpp" + "src/discovery/SPDPAgent.cpp" + "src/discovery/TopicData.cpp" + "src/entities/Domain.cpp" + "src/entities/Participant.cpp" + "src/entities/Reader.cpp" + "src/entities/StatelessReader.cpp" + "src/entities/Writer.cpp" + "src/messages/MessageReceiver.cpp" + "src/messages/MessageTypes.cpp" + "src/utils/Diagnostics.cpp" + "thirdparty/Micro-CDR/src/c/common.c" + "thirdparty/Micro-CDR/src/c/types/array.c" + "thirdparty/Micro-CDR/src/c/types/basic.c" + "thirdparty/Micro-CDR/src/c/types/sequence.c" + "thirdparty/Micro-CDR/src/c/types/string.c" + INCLUDE_DIRS + "include" + "thirdparty/Micro-CDR/include" + REQUIRES + base_component cdr task thread_pool socket +) + +# target_compile_features(${COMPONENT_LIB} PUBLIC cxx_std_17) +# target_compile_definitions(${COMPONENT_LIB} PUBLIC RTPS_CONFIG_HEADER=\"rtps/config_esp32.h\") diff --git a/components/rtps_embedded/README.md b/components/rtps_embedded/README.md new file mode 100644 index 000000000..45d161076 --- /dev/null +++ b/components/rtps_embedded/README.md @@ -0,0 +1,162 @@ +# rtps_embedded + +ESPP component that integrates the [embeddedRTPS](https://github.com/embedded-software-laboratory/embeddedRTPS) +RTPS/DDS stack into the ESPP ecosystem. +Any platform that can build ESPP — including ESP32, Linux, and desktop PCs — +can use this component to discover and exchange typed messages with ROS 2 nodes +or any other DDS participant on the same network using the standard RTPS wire +protocol. + +The original embeddedRTPS library has hard dependencies on FreeRTOS and lwIP. +`rtps_embedded` removes those dependencies by replacing all socket, task, and +synchronisation calls with ESPP's platform-agnostic `UdpSocket`, `Task`, and +`ThreadPool` primitives. When built for ESP32, ESPP uses FreeRTOS and lwIP +under the hood; on other platforms it uses the host OS equivalents — the RTPS +code itself is unchanged in either case. + +--- + +## Architecture + +``` +user code + │ + ▼ +rtps::Domain — routes packets to participants; owns discovery threads + │ + ├── rtps::Participant — groups writers and readers + │ ├── rtps::Writer — publishes CacheChange samples + │ └── rtps::Reader — delivers samples to a user callback + │ + ├── rtps::ThreadPool — espp::ThreadPool workers that drain the four + │ incoming/outgoing meta/user traffic queues + │ + └── rtps::EsppTransport — one espp::UdpSocket per open UDP port, + each with its own receive task +``` + +`EsppTransport` is the sole platform-specific adapter. It wraps ESPP's +`UdpSocket` and `Task`, which in turn map to: + +| Build target | Socket backend | Task backend | +|---|---|---| +| ESP32 | lwIP (via ESP-IDF) | FreeRTOS | +| Linux / PC | POSIX sockets | `std::thread` | + +--- + +## Quick-start + +```cpp +#include "rtps/entities/Domain.h" + +// 1. Construct the domain with the local interface IP. +rtps::Domain domain(local_ip); + +// 2. Create a participant *before* completeInit(). +rtps::Participant *part = domain.createParticipant(); + +// 3. Add user-defined writer and reader endpoints. +rtps::Writer *writer = domain.createWriter(*part, "my/topic", + "std_msgs::msg::String", false); +rtps::Reader *reader = domain.createReader(*part, "my/topic", + "std_msgs::msg::String", false); + +// 4. Register a receive callback on the reader. +reader->registerCallback( + [](void *, const rtps::ReaderCacheChange &change) { + // process change.getData() / change.copyInto(...) + }, nullptr); + +// 5. Start discovery (SPDP/SEDP) and worker threads. +domain.completeInit(); + +// 6. Publish a sample. +const char *payload = "hello"; +writer->newChange(rtps::ChangeKind_t::ALIVE, + reinterpret_cast(payload), + static_cast(strlen(payload) + 1)); +``` + +> **Note**: `createParticipant()` **must** be called before `completeInit()`. +> No new participants can be added after init is complete. + +--- + +## Configuration + +Two built-in config headers are provided. Select one by defining +`RTPS_CONFIG_HEADER`, or let `include/rtps/config.h` pick automatically based +on the build target. + +| Header | Target | +|---|---| +| [`include/rtps/config_esp32.h`](include/rtps/config_esp32.h) | ESP32 (ESP-IDF) | +| [`include/rtps/config_desktop.h`](include/rtps/config_desktop.h) | Linux / PC | + +All tunable constants follow the same layout in both files: + +| Constant | Default | Description | +|---|---|---| +| `DOMAIN_ID` | 0 | RTPS domain number (0–230 with UDP) | +| `MAX_NUM_PARTICIPANTS` | 1 | Participant pool size | +| `NUM_STATEFUL_WRITERS` | 5 | User writer endpoint pool | +| `NUM_STATEFUL_READERS` | 5 | User reader endpoint pool | +| `NUM_STATELESS_WRITERS` | 5 | Discovery writer endpoint pool | +| `NUM_STATELESS_READERS` | 5 | Discovery reader endpoint pool | +| `NUM_WRITERS_PER_PARTICIPANT` | 10 | Max writers per participant | +| `NUM_READERS_PER_PARTICIPANT` | 10 | Max readers per participant | +| `HISTORY_SIZE_STATEFUL` | 10 | Per-endpoint history depth | +| `THREAD_POOL_NUM_WRITERS` | 2 | Writer worker threads | +| `THREAD_POOL_NUM_READERS` | 2 | Reader worker threads | +| `THREAD_POOL_WRITER_STACKSIZE` | 4096 B | Writer task stack | +| `THREAD_POOL_READER_STACKSIZE` | 6144 B | Reader / UDP-receive task stack | +| `MAX_NUM_UDP_CONNECTIONS` | 10 | UDP socket pool size | +| `SPDP_RESEND_PERIOD_MS` | 2000 | Discovery announce period | +| `SF_WRITER_HB_PERIOD_MS` | 4000 | Reliable-writer heartbeat period | + +The `OVERALL_HEAP_SIZE` constant at the bottom of that file estimates the +total stack RAM consumed by all internal tasks. + +--- + +## ESPP component dependencies + +| Component | Purpose | +|---|---| +| `base_component` | ESPP base class with integrated `espp::Logger` | +| `socket` | ESPP `UdpSocket` used by `EsppTransport` | +| `task` | ESPP `Task` for per-port UDP receive loops | +| `thread_pool` | ESPP `ThreadPool` for writer/reader workers | +| `cdr` | CDR serialization helpers | + +These components abstract away all OS and network-stack details, so +`rtps_embedded` itself has no direct dependency on FreeRTOS, lwIP, or any +other platform library. + +Third-party (vendored in `thirdparty/`): + +| Library | Purpose | +|---|---| +| `Micro-CDR` | eProsima CDR (de)serialization for discovery messages | + +--- + +## Example + +See [`example/`](example/) for a two-node **initiator / responder** demo. + +The same logic runs on any ESPP-supported platform. For ESP32, flash one board +as *Initiator* and a second as *Responder* via menuconfig +(`idf.py menuconfig → RTPS Example Configuration`). The initiator periodically +publishes numbered request messages; the responder echoes each message back on +the response topic. + +Key menuconfig options (ESP32 example): + +| Option | Description | +|---|---| +| `RTPS_EXAMPLE_ROLE` | `Initiator` or `Responder` | +| `RTPS_EXAMPLE_TOPIC_PREFIX` | Shared topic prefix (e.g. `espp/rtps_example`) | +| `RTPS_EXAMPLE_PUBLISH_PERIOD_MS` | Initiator publish interval | +| `ESP_WIFI_SSID` / `ESP_WIFI_PASSWORD` | Wi-Fi credentials | diff --git a/components/rtps_embedded/example/CMakeLists.txt b/components/rtps_embedded/example/CMakeLists.txt new file mode 100644 index 000000000..b65580252 --- /dev/null +++ b/components/rtps_embedded/example/CMakeLists.txt @@ -0,0 +1,21 @@ +# The following lines of boilerplate have to be in your project's CMakeLists +# in this exact order for cmake to work correctly +cmake_minimum_required(VERSION 3.20) + +set(ENV{IDF_COMPONENT_MANAGER} "0") +include($ENV{IDF_PATH}/tools/cmake/project.cmake) + +set(EXTRA_COMPONENT_DIRS + "../../../components/" +) + +set( + COMPONENTS + "main esptool_py logger rtps_embedded wifi" + CACHE STRING + "List of components to include" + ) + +project(rtps_embedded_example) + +set(CMAKE_CXX_STANDARD 20) diff --git a/components/rtps_embedded/example/main/CMakeLists.txt b/components/rtps_embedded/example/main/CMakeLists.txt new file mode 100644 index 000000000..71403499e --- /dev/null +++ b/components/rtps_embedded/example/main/CMakeLists.txt @@ -0,0 +1,3 @@ +idf_component_register(SRC_DIRS "." + INCLUDE_DIRS "." + REQUIRES logger rtps_embedded wifi) diff --git a/components/rtps_embedded/example/main/Kconfig.projbuild b/components/rtps_embedded/example/main/Kconfig.projbuild new file mode 100644 index 000000000..7d4a64dbc --- /dev/null +++ b/components/rtps_embedded/example/main/Kconfig.projbuild @@ -0,0 +1,109 @@ +menu "RTPS Example Configuration" + + choice RTPS_EXAMPLE_ROLE + prompt "Example Role" + default RTPS_EXAMPLE_ROLE_INITIATOR + help + Build one board as the initiator and a second board as the responder + to verify RTPS discovery and end-to-end UInt32 sample exchange. + + config RTPS_EXAMPLE_ROLE_INITIATOR + bool "Initiator" + help + Publishes incrementing request samples after it discovers a + responder, then logs the matching responses. + + config RTPS_EXAMPLE_ROLE_RESPONDER + bool "Responder" + help + Waits for request samples and echoes the same value back on the + response topic. + + endchoice + + config RTPS_EXAMPLE_NODE_NAME + string "Participant node name" + default "espp_rtps_initiator" if RTPS_EXAMPLE_ROLE_INITIATOR + default "espp_rtps_responder" if RTPS_EXAMPLE_ROLE_RESPONDER + help + Logical RTPS participant name announced during discovery. + + config RTPS_EXAMPLE_DOMAIN_ID + int "RTPS domain ID" + range 0 231 + default 0 + help + Both boards must use the same domain ID to discover each other. + + config RTPS_EXAMPLE_PARTICIPANT_ID + int "RTPS participant ID" + range 0 119 + default 1 if RTPS_EXAMPLE_ROLE_INITIATOR + default 2 if RTPS_EXAMPLE_ROLE_RESPONDER + help + Each board should use a unique participant ID within the same domain. + + config RTPS_EXAMPLE_TOPIC_PREFIX + string "Topic prefix" + default "espp/rtps_example" + help + Prefix used to derive the request and response topics. + + config RTPS_EXAMPLE_ANNOUNCE_PERIOD_MS + int "Discovery announce period (ms)" + range 200 10000 + default 1500 + help + Period between periodic SPDP/SEDP discovery announcements. + + config RTPS_EXAMPLE_PUBLISH_PERIOD_MS + int "Initiator publish period (ms)" + range 250 60000 + default 2000 + depends on RTPS_EXAMPLE_ROLE_INITIATOR + help + Period between request messages sent by the initiator after a + responder has been discovered. + + config RTPS_EXAMPLE_USE_USER_MULTICAST + bool "Use best-effort user-data multicast" + default n + help + If enabled, the example advertises and uses topic-specific + multicast groups for the request and response topics instead of + per-participant unicast delivery. + + config RTPS_EXAMPLE_REQUEST_MULTICAST_GROUP + string "Request topic multicast group" + default "239.255.0.11" + depends on RTPS_EXAMPLE_USE_USER_MULTICAST + help + IPv4 multicast group used for the /request topic. + + config RTPS_EXAMPLE_RESPONSE_MULTICAST_GROUP + string "Response topic multicast group" + default "239.255.0.12" + depends on RTPS_EXAMPLE_USE_USER_MULTICAST + help + IPv4 multicast group used for the /response topic. + + config ESP_WIFI_SSID + string "WiFi SSID" + default "" + help + SSID (network name) for the example to connect to. + + config ESP_WIFI_PASSWORD + string "WiFi Password" + default "" + help + WiFi password (WPA or WPA2) for the example to use. + + config ESP_MAXIMUM_RETRY + int "Maximum retry" + default 5 + help + Set the maximum retry count to avoid reconnecting forever when the + network is unavailable. + +endmenu diff --git a/components/rtps_embedded/example/main/main.cpp b/components/rtps_embedded/example/main/main.cpp new file mode 100644 index 000000000..4b0ebe748 --- /dev/null +++ b/components/rtps_embedded/example/main/main.cpp @@ -0,0 +1,176 @@ + +#include +#include +#include + +#include "esp_log.h" +#include "freertos/FreeRTOS.h" +#include "freertos/task.h" + +#include "rtps/entities/Domain.h" +#include "logger.hpp" +#include "wifi_sta.hpp" + +using namespace std::chrono_literals; + +namespace { + +static const char *TAG = "rtps_example"; +static bool s_started = false; +static rtps::Domain *s_domain = nullptr; +static rtps::Participant *s_participant = nullptr; +static rtps::Writer *s_writer = nullptr; +static rtps::Reader *s_reader = nullptr; + +bool send_text_message(const char *text) { + if (text == nullptr || s_writer == nullptr) { + return false; + } + + const size_t len = strnlen(text, 127); + const rtps::CacheChange *change = s_writer->newChange( + rtps::ChangeKind_t::ALIVE, reinterpret_cast(text), + static_cast(len + 1)); + + return change != nullptr; +} + +void reader_cb(void * /*callee*/, const rtps::ReaderCacheChange &change) { + char buffer[128] = {0}; + const rtps::DataSize_t copy_len = + (change.getDataSize() < static_cast(sizeof(buffer) - 1)) + ? change.getDataSize() + : static_cast(sizeof(buffer) - 1); + + if (copy_len == 0 || + !change.copyInto(reinterpret_cast(buffer), sizeof(buffer))) { + return; + } + + ESP_LOGI(TAG, "rx (%u B): %s", static_cast(copy_len), buffer); + +#if CONFIG_RTPS_EXAMPLE_ROLE_RESPONDER + // Echo the message back on the publish topic. + if (!send_text_message(buffer)) { + ESP_LOGW(TAG, "tx echo dropped (history full or no matched reader)"); + } +#endif +} + +#if CONFIG_RTPS_EXAMPLE_ROLE_INITIATOR +void publisher_task(void * /*arg*/) { + uint32_t counter = 0; + while (true) { + char msg[32]; + snprintf(msg, sizeof(msg), "request %u", static_cast(counter++)); + if (!send_text_message(msg)) { + ESP_LOGW(TAG, "tx dropped (history full or no matched reader)"); + } else { + ESP_LOGI(TAG, "tx: %s", msg); + } + vTaskDelay(pdMS_TO_TICKS(CONFIG_RTPS_EXAMPLE_PUBLISH_PERIOD_MS)); + } +} +#endif + +} // namespace + +// Start the RTPS stack. Topics are derived from the configured prefix: +// initiator publishes on /request, subscribes to /response +// responder publishes on /response, subscribes to /request +extern "C" void embedded_rtps_start(const rtps::Ip4AddressBytes &local_ip) { + if (s_started) { + return; + } + + const std::string prefix = CONFIG_RTPS_EXAMPLE_TOPIC_PREFIX; +#if CONFIG_RTPS_EXAMPLE_ROLE_INITIATOR + const std::string pub_topic = prefix + "/request"; + const std::string sub_topic = prefix + "/response"; +#else + const std::string pub_topic = prefix + "/response"; + const std::string sub_topic = prefix + "/request"; +#endif + + static rtps::Domain domain(local_ip); + s_domain = &domain; + + // Participants must be created before completeInit() starts the discovery + // threads; no new participants can be added after that point. + s_participant = s_domain->createParticipant(); + if (s_participant == nullptr) { + ESP_LOGE(TAG, "Failed to create RTPS participant"); + return; + } + + if (!s_domain->completeInit()) { + ESP_LOGE(TAG, "Failed to complete RTPS domain init"); + return; + } + + s_writer = s_domain->createWriter(*s_participant, pub_topic.c_str(), + "std_msgs::msg::String", false); + if (s_writer == nullptr) { + ESP_LOGE(TAG, "Failed to create RTPS writer"); + return; + } + + s_reader = s_domain->createReader(*s_participant, sub_topic.c_str(), + "std_msgs::msg::String", false); + if (s_reader == nullptr) { + ESP_LOGE(TAG, "Failed to create RTPS reader"); + return; + } + + if (s_reader->registerCallback(reader_cb, nullptr) == 0) { + ESP_LOGE(TAG, "Failed to register RTPS reader callback"); + return; + } + +#if CONFIG_RTPS_EXAMPLE_ROLE_INITIATOR + xTaskCreate(publisher_task, "rtps_pub", 4096, nullptr, 5, nullptr); +#endif + + s_started = true; + ESP_LOGI(TAG, "started as '%s': pub=%s sub=%s", + CONFIG_RTPS_EXAMPLE_NODE_NAME, pub_topic.c_str(), sub_topic.c_str()); +} + + +extern "C" void app_main(void) { + espp::Logger logger({.tag = TAG, .level = espp::Logger::Verbosity::INFO}); + + //! [rtps example] + std::string ip_address; + espp::WifiSta wifi_sta({ + .ssid = CONFIG_ESP_WIFI_SSID, + .password = CONFIG_ESP_WIFI_PASSWORD, + .num_connect_retries = CONFIG_ESP_MAXIMUM_RETRY, + .on_connected = nullptr, + .on_disconnected = nullptr, + .on_got_ip = [&ip_address](ip_event_got_ip_t *eventdata) { + ip_address = fmt::format("{}.{}.{}.{}", IP2STR(&eventdata->ip_info.ip)); + fmt::print("got IP: {}\n", ip_address); + }, + }); + + logger.info("Waiting for WiFi connection..."); + while (!wifi_sta.is_connected()) { + std::this_thread::sleep_for(100ms); + } + logger.info("WiFi connected, local IP {}", ip_address); + + rtps::Ip4AddressBytes local_ip{0, 0, 0, 0}; + if (std::sscanf(ip_address.c_str(), "%hhu.%hhu.%hhu.%hhu", + &local_ip[0], &local_ip[1], &local_ip[2], &local_ip[3]) != 4) { + logger.error("Failed to parse local IP {}", ip_address); + return; + } + + embedded_rtps_start(local_ip); + //! [rtps example] + + while (true) { + vTaskDelay(pdMS_TO_TICKS(1000)); + } +} diff --git a/components/rtps_embedded/example/partitions.csv b/components/rtps_embedded/example/partitions.csv new file mode 100644 index 000000000..c4217ab9e --- /dev/null +++ b/components/rtps_embedded/example/partitions.csv @@ -0,0 +1,5 @@ +# ESP-IDF Partition Table +# Name, Type, SubType, Offset, Size, Flags +nvs, data, nvs, 0x9000, 0x6000, +phy_init, data, phy, 0xf000, 0x1000, +factory, app, factory, 0x10000, 1500K, diff --git a/components/rtps_embedded/example/sdkconfig.defaults b/components/rtps_embedded/example/sdkconfig.defaults new file mode 100644 index 000000000..e823df850 --- /dev/null +++ b/components/rtps_embedded/example/sdkconfig.defaults @@ -0,0 +1,13 @@ +# Common ESP-related +# +CONFIG_ESP_SYSTEM_EVENT_TASK_STACK_SIZE=4096 +CONFIG_ESP_MAIN_TASK_STACK_SIZE=8192 + +# +# Partition Table +# +CONFIG_PARTITION_TABLE_CUSTOM=y +CONFIG_PARTITION_TABLE_CUSTOM_FILENAME="partitions.csv" +CONFIG_PARTITION_TABLE_FILENAME="partitions.csv" +CONFIG_PARTITION_TABLE_OFFSET=0x8000 +CONFIG_PARTITION_TABLE_MD5=y diff --git a/components/rtps_embedded/include/rtps/ThreadPool.h b/components/rtps_embedded/include/rtps/ThreadPool.h new file mode 100644 index 000000000..7d8d09d6b --- /dev/null +++ b/components/rtps_embedded/include/rtps/ThreadPool.h @@ -0,0 +1,109 @@ +/* +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_THREADPOOL_H +#define RTPS_THREADPOOL_H + +#include "base_component.hpp" +#include "rtps/communication/PacketInfo.h" +#include "rtps/common/types.h" +#include "rtps/config.h" +#include "rtps/storages/ThreadSafeCircularBuffer.h" + +#include +#include +#include + +namespace rtps { + +class Writer; + +} // namespace rtps + +namespace espp { +class ThreadPool; +} + +namespace rtps { + +class ThreadPool : public espp::BaseComponent { +public: + using receiveJumppad_fp = void (*)(void *callee, const PacketInfo &packet); + + ThreadPool(receiveJumppad_fp receiveCallback, void *callee); + + ~ThreadPool(); + + bool startThreads(); + void stopThreads(); + + void clearQueues(); + bool addWorkload(Writer *workload); + bool addNewPacket(PacketInfo &&packet); + + static void onDatagram( + void *arg, const uint8_t *data, std::size_t size, Ip4Port_t localPort, + Ip4Port_t remotePort, + const Ip4AddressBytes &remoteAddress); + + bool addBuiltinPort(const Ip4Port_t &port); + +private: + receiveJumppad_fp m_receiveJumppad; + void *m_callee; + bool m_running = false; + std::unique_ptr m_writerWorkers; + std::unique_ptr m_readerWorkers; + + std::array m_builtinPorts; + size_t m_builtinPortsIdx = 0; + + void updateDiagnostics(); + + using BufferUsertrafficOutgoing = ThreadSafeCircularBuffer< + Writer *, Config::THREAD_POOL_WORKLOAD_QUEUE_LENGTH_USERTRAFFIC>; + using BufferMetatrafficOutgoing = ThreadSafeCircularBuffer< + Writer *, Config::THREAD_POOL_WORKLOAD_QUEUE_LENGTH_METATRAFFIC>; + using BufferUsertrafficIncoming = ThreadSafeCircularBuffer< + PacketInfo, Config::THREAD_POOL_WORKLOAD_QUEUE_LENGTH_USERTRAFFIC>; + using BufferMetatrafficIncoming = ThreadSafeCircularBuffer< + PacketInfo, Config::THREAD_POOL_WORKLOAD_QUEUE_LENGTH_METATRAFFIC>; + + BufferUsertrafficOutgoing m_outgoingUserTraffic; + BufferMetatrafficOutgoing m_outgoingMetaTraffic; + + BufferUsertrafficIncoming m_incomingUserTraffic; + BufferMetatrafficIncoming m_incomingMetaTraffic; + + void scheduleWriterWork(); + void scheduleReaderWork(); + + bool isBuiltinPort(const Ip4Port_t &port); + void doWriterWork(); + void doReaderWork(); +}; +} // namespace rtps + +#endif // RTPS_THREADPOOL_H diff --git a/components/rtps_embedded/include/rtps/common/Locator.h b/components/rtps_embedded/include/rtps/common/Locator.h new file mode 100644 index 000000000..fc1bdc36f --- /dev/null +++ b/components/rtps_embedded/include/rtps/common/Locator.h @@ -0,0 +1,208 @@ +/* +The MIT License +Copyright (c) 2019 Lehrstuhl Informatik 11 - RWTH Aachen University +Modifications Copyright (c) 2026 ATDev +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: +The above copyright notice and this permission notice shall be included in +all copies or substantial portions of the Software. +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +THE SOFTWARE + +This file is part of embeddedRTPS. + +Author: i11 - Embedded Software, RWTH Aachen University +*/ + +#ifndef RTPS_LOCATOR_T_H +#define RTPS_LOCATOR_T_H + +#include "rtps/common/types.h" +#include "rtps/utils/udpUtils.h" +#include "ucdr/microcdr.h" + +#include + +namespace rtps { + +#if defined(_MSC_VER) +#define RTPS_EMBEDDED_PACKED +#pragma pack(push, 1) +#else +#define RTPS_EMBEDDED_PACKED __attribute__((packed)) +#endif + +inline bool isSameSubnetAddress( + const std::array &addr, + const std::array &local) { + return addr[0] == local[0] && addr[1] == local[1] && addr[2] == local[2]; +} + +enum class LocatorKind_t : int32_t { + LOCATOR_KIND_INVALID = -1, + LOCATOR_KIND_RESERVED = 0, + LOCATOR_KIND_UDPv4 = 1, + LOCATOR_KIND_UDPv6 = 2 +}; + +const uint32_t LOCATOR_PORT_INVALID = 0; +const std::array LOCATOR_ADDRESS_INVALID = { + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}; + +/* + * This representation corresponds to the RTPS wire format + */ +struct FullLengthLocator { + LocatorKind_t kind = LocatorKind_t::LOCATOR_KIND_INVALID; + uint32_t port = LOCATOR_PORT_INVALID; + std::array address = + LOCATOR_ADDRESS_INVALID; // TODO make private such that kind and address + // always match? + + static FullLengthLocator createUDPv4Locator(uint8_t a, uint8_t b, uint8_t c, + uint8_t d, uint32_t port) { + FullLengthLocator locator; + locator.kind = LocatorKind_t::LOCATOR_KIND_UDPv4; + locator.address = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, a, b, c, d}; + locator.port = port; + return locator; + } + + void setInvalid() { kind = LocatorKind_t::LOCATOR_KIND_INVALID; } + + bool isValid() const { return kind != LocatorKind_t::LOCATOR_KIND_INVALID; } + + bool readFromUcdrBuffer(ucdrBuffer &buffer) { + if (ucdr_buffer_remaining(&buffer) < sizeof(FullLengthLocator)) { + return false; + } else { + ucdr_deserialize_array_uint8_t(&buffer, reinterpret_cast(this), + sizeof(FullLengthLocator)); + return true; + } + } + + bool serializeIntoUdcrBuffer(ucdrBuffer &buffer) { + if (ucdr_buffer_remaining(&buffer) < sizeof(FullLengthLocator)) { + return false; + } else { + ucdr_serialize_array_uint8_t(&buffer, reinterpret_cast(this), + sizeof(FullLengthLocator)); + } + } + + std::array getIp4Address() const { + return getIp4AddressBytes(); + } + + std::array getIp4AddressBytes() const { + return {address[12], address[13], address[14], address[15]}; + } + + bool isSameAddress(const std::array &ipAddress) { + return getIp4AddressBytes() == ipAddress; + } + + inline bool isSameSubnet(const std::array &localIp) const { + return isSameSubnetAddress(getIp4AddressBytes(), localIp); + } + + inline bool isMulticastAddress() const { + const auto ip = getIp4AddressBytes(); + return ip[0] >= 224 && ip[0] <= 239; + } + + inline uint32_t getLocatorPort() { return static_cast(port); } + +} RTPS_EMBEDDED_PACKED; + +inline FullLengthLocator +getBuiltInUnicastLocator(ParticipantId_t participantId, + const std::array &localIp) { + return FullLengthLocator::createUDPv4Locator( + localIp[0], localIp[1], localIp[2], localIp[3], + getBuiltInUnicastPort(participantId)); +} + +inline FullLengthLocator getBuiltInMulticastLocator() { + return FullLengthLocator::createUDPv4Locator(239, 255, 0, 1, + getBuiltInMulticastPort()); +} + +inline FullLengthLocator +getUserUnicastLocator(ParticipantId_t participantId, + const std::array &localIp) { + return FullLengthLocator::createUDPv4Locator( + localIp[0], localIp[1], localIp[2], localIp[3], + getUserUnicastPort(participantId)); +} + +inline FullLengthLocator +getUserMulticastLocator( + const std::array &localIp) { // this would be a unicastaddress, + // as defined in config + return FullLengthLocator::createUDPv4Locator( + localIp[0], localIp[1], localIp[2], localIp[3], + getUserMulticastPort()); +} + +inline FullLengthLocator getDefaultSendMulticastLocator() { + return FullLengthLocator::createUDPv4Locator(239, 255, 0, 1, + getBuiltInMulticastPort()); +} + +/* + * This representation omits unnecessary 12 bytes of the full RTPS wire format + */ +struct LocatorIPv4 { + LocatorKind_t kind = LocatorKind_t::LOCATOR_KIND_INVALID; + std::array address = {0}; + uint32_t port = LOCATOR_PORT_INVALID; + + LocatorIPv4() = default; + LocatorIPv4(const FullLengthLocator &locator) { + address[0] = locator.address[12]; + address[1] = locator.address[13]; + address[2] = locator.address[14]; + address[3] = locator.address[15]; + port = locator.port; + kind = locator.kind; + } + + std::array getIp4Address() const { + return getIp4AddressBytes(); + } + + std::array getIp4AddressBytes() const { return address; } + + void setInvalid() { kind = LocatorKind_t::LOCATOR_KIND_INVALID; } + + bool isValid() const { return kind != LocatorKind_t::LOCATOR_KIND_INVALID; } + + inline bool isSameSubnet(const std::array &localIp) const { + return isSameSubnetAddress(getIp4AddressBytes(), localIp); + } + + inline bool isMulticastAddress() const { + const auto ip = getIp4AddressBytes(); + return ip[0] >= 224 && ip[0] <= 239; + } +}; + +} // namespace rtps + +#if defined(_MSC_VER) +#pragma pack(pop) +#endif +#undef RTPS_EMBEDDED_PACKED + +#endif // RTPS_LOCATOR_T_H diff --git a/components/rtps_embedded/include/rtps/common/types.h b/components/rtps_embedded/include/rtps/common/types.h new file mode 100644 index 000000000..f6488c79a --- /dev/null +++ b/components/rtps_embedded/include/rtps/common/types.h @@ -0,0 +1,304 @@ +/* +The MIT License +Copyright (c) 2019 Lehrstuhl Informatik 11 - RWTH Aachen University +Modifications Copyright (c) 2026 ATDev +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: +The above copyright notice and this permission notice shall be included in +all copies or substantial portions of the Software. +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +THE SOFTWARE + +This file is part of embeddedRTPS. + +Author: i11 - Embedded Software, RWTH Aachen University +*/ + +#ifndef RTPS_TYPES_H +#define RTPS_TYPES_H + +#include +#include +#include +#include +#include + +// TODO subnamespaces +namespace rtps { + +// TODO move types to where they are needed! + +typedef uint16_t Ip4Port_t; +typedef uint16_t DataSize_t; +typedef int8_t ParticipantId_t; // With UDP only 120 possible + +enum class EntityKind_t : uint8_t { + USER_DEFINED_UNKNOWN = 0x00, + // No user define participant + USER_DEFINED_WRITER_WITH_KEY = 0x02, + USER_DEFINED_WRITER_WITHOUT_KEY = 0x03, + USER_DEFINED_READER_WITHOUT_KEY = 0x04, + USER_DEFINED_READER_WITH_KEY = 0x07, + + BUILD_IN_UNKNOWN = 0xc0, + BUILD_IN_PARTICIPANT = 0xc1, + BUILD_IN_WRITER_WITH_KEY = 0xc2, + BUILD_IN_WRITER_WITHOUT_KEY = 0xc3, + BUILD_IN_READER_WITHOUT_KEY = 0xc4, + BUILD_IN_READER_WITH_KEY = 0xc7, + + VENDOR_SPEC_UNKNOWN = 0x40, + VENDOR_SPEC_PARTICIPANT = 0x41, + VENDOR_SPEC_WRITER_WITH_KEY = 0x42, + VENDOR_SPEC_WRITER_WITHOUT_KEY = 0x43, + VENDOR_SPEC_READER_WITHOUT_KEY = 0x44, + VENDOR_SPEC_READER_WITH_KEY = 0x47 +}; + +enum class TopicKind_t : uint8_t { NO_KEY = 1, WITH_KEY = 2 }; + +enum class ChangeKind_t : uint8_t { + INVALID, + ALIVE, + NOT_ALIVE_DISPOSED, + NOT_ALIVE_UNREGISTERED +}; + +enum class ReliabilityKind_t : uint32_t { + BEST_EFFORT = 1, + RELIABLE = 2 // Specification says 3 but eprosima sends 2 +}; + +enum class DurabilityKind_t : uint32_t { + VOLATILE = 0, + TRANSIENT_LOCAL = 1, + TRANSIENT = 2, + PERSISTENT = 3 +}; + +struct GuidPrefix_t { + std::array id; + + bool operator==(const GuidPrefix_t &other) const { + return this->id == other.id; + } +}; + +struct EntityId_t { + std::array entityKey; + EntityKind_t entityKind; + + bool operator==(const EntityId_t &other) const { + return this->entityKey == other.entityKey && + this->entityKind == other.entityKind; + } + + bool operator!=(const EntityId_t &other) const { return !(*this == other); } +}; + +struct Guid_t { + GuidPrefix_t prefix; + EntityId_t entityId; + + bool operator==(const Guid_t &other) const { + return this->prefix == other.prefix && this->entityId == other.entityId; + } + + static uint32_t sum(const Guid_t &other) { + uint32_t ret = 0; + for (const auto &i : other.prefix.id) { + ret += i; + } + for (const auto &i : other.entityId.entityKey) { + ret += i; + } + return ret; + } +}; + +// Described as long but there wasn't any definition. Other than 32 bit does not +// conform the default values +struct Time_t { + int32_t seconds; // time in seconds + uint32_t fraction; // time in sec/2^32 (?) + + static Time_t create(int32_t s, uint32_t ns) { + static constexpr double factor = + (static_cast(1) << 32) / 1000000000.; + auto fraction = static_cast(ns * factor); + return Time_t{s, fraction}; + } +}; + +struct VendorId_t { + std::array vendorId; +}; + +struct SequenceNumber_t { + int32_t high; + uint32_t low; + + bool operator==(const SequenceNumber_t &other) const { + return high == other.high && low == other.low; + } + + bool operator!=(const SequenceNumber_t &other) const { + return !(*this == other); + } + + bool operator<(const SequenceNumber_t &other) const { + return high < other.high || (high == other.high && low < other.low); + } + + bool operator>(const SequenceNumber_t &other) const { + return high > other.high || (high == other.high && low > other.low); + } + + bool operator<=(const SequenceNumber_t &other) const { + return *this == other || *this < other; + } + + SequenceNumber_t &operator++() { + ++low; + if (low == 0) { + ++high; + } + return *this; + } + + SequenceNumber_t &operator--() { + if (low == 0) { + --high; + low = std::numeric_limits::max(); + } else { + --low; + } + + return *this; + } + + SequenceNumber_t operator++(int) { + SequenceNumber_t tmp(*this); + ++*this; + return tmp; + } +}; + +#define SNS_MAX_NUM_BITS 256 +#define SNS_NUM_BYTES (SNS_MAX_NUM_BITS / 8) +static_assert(!(SNS_MAX_NUM_BITS % 32) && SNS_MAX_NUM_BITS != 0, + "SNS_MAX_NUM_BITS must be multiple of 32"); + +struct SequenceNumberSet { + + SequenceNumberSet() = default; + explicit SequenceNumberSet(const SequenceNumber_t &firstMissing) + : base(firstMissing) {} + + SequenceNumber_t base = {0, 0}; + // Cannot be static because of packed + uint32_t numBits = 0; + std::array bitMap{}; + + // We only need 1 byte because atm we don't store packets. + bool isSet(uint32_t bit) const { + if (bit >= SNS_MAX_NUM_BITS) { + return true; + } + const auto bucket = static_cast(bit / 32); + const auto pos = static_cast(bit % 32); + return (bitMap[bucket] & (1 << (31 - pos))) != 0; + } +}; + +struct FragmentNumber_t { + uint32_t value; +}; + +struct Count_t { + int32_t value; +}; + +struct ProtocolVersion_t { + uint8_t major; + uint8_t minor; +}; + +typedef Time_t Duration_t; // TODO + +enum class ChangeForReaderStatusKind { + UNSENT, + UNACKNOWLEDGED, + REQURESTED, + ACKNOWLEDGED, + UNDERWAY +}; + +enum class ChangeFromWriterStatusKind { LOST, MISSING, RECEIVED, UNKNOWN }; + +struct InstanceHandle_t { // TODO + uint64_t value; +}; + +struct ParticipantMessageData { // TODO +}; + +using Ip4AddressBytes = std::array; + +using ReceiveCallback = void (*)(void *arg, const uint8_t *data, + std::size_t size, Ip4Port_t localPort, + Ip4Port_t remotePort, + const Ip4AddressBytes &remoteAddress); + +/* Default Values */ +const EntityId_t ENTITYID_UNKNOWN{}; +const EntityId_t ENTITYID_BUILD_IN_PARTICIPANT = { + {00, 00, 01}, EntityKind_t::BUILD_IN_PARTICIPANT}; +const EntityId_t ENTITYID_SEDP_BUILTIN_TOPIC_WRITER = { + {00, 00, 02}, EntityKind_t::BUILD_IN_WRITER_WITH_KEY}; +const EntityId_t ENTITYID_SEDP_BUILTIN_TOPIC_READER = { + {00, 00, 02}, EntityKind_t::BUILD_IN_READER_WITH_KEY}; +const EntityId_t ENTITYID_SEDP_BUILTIN_PUBLICATIONS_WRITER = { + {00, 00, 03}, EntityKind_t::BUILD_IN_WRITER_WITH_KEY}; +const EntityId_t ENTITYID_SEDP_BUILTIN_PUBLICATIONS_READER = { + {00, 00, 03}, EntityKind_t::BUILD_IN_READER_WITH_KEY}; +const EntityId_t ENTITYID_SEDP_BUILTIN_SUBSCRIPTIONS_WRITER = { + {00, 00, 04}, EntityKind_t::BUILD_IN_WRITER_WITH_KEY}; +const EntityId_t ENTITYID_SEDP_BUILTIN_SUBSCRIPTIONS_READER = { + {00, 00, 04}, EntityKind_t::BUILD_IN_READER_WITH_KEY}; +const EntityId_t ENTITYID_SPDP_BUILTIN_PARTICIPANT_WRITER = { + {00, 01, 00}, EntityKind_t::BUILD_IN_WRITER_WITH_KEY}; +const EntityId_t ENTITYID_SPDP_BUILTIN_PARTICIPANT_READER = { + {00, 01, 00}, EntityKind_t::BUILD_IN_READER_WITH_KEY}; +const EntityId_t ENTITYID_P2P_BUILTIN_PARTICIPANT_MESSAGE_WRITER = { + {00, 02, 00}, EntityKind_t::BUILD_IN_WRITER_WITH_KEY}; +const EntityId_t ENTITYID_P2P_BUILTIN_PARTICIPANT_MESSAGE_READER = { + {00, 02, 00}, EntityKind_t::BUILD_IN_READER_WITH_KEY}; + +const GuidPrefix_t GUIDPREFIX_UNKNOWN{}; +const Guid_t GUID_UNKNOWN{}; +const GuidPrefix_t GUID_RANDOM{9, 9, 9, 9, 9, 9, 9, 9, 9, 9, 9}; + +const ParticipantId_t PARTICIPANT_ID_INVALID = -1; + +const ProtocolVersion_t PROTOCOLVERSION = {2, 2}; + +const SequenceNumber_t SEQUENCENUMBER_UNKNOWN = {-1, 0}; + +const Time_t TIME_ZERO = {}; +const Time_t TIME_INVALID = {-1, 0xFFFFFFFF}; +const Time_t TIME_INFINITY = {0x7FFFFFFF, 0xFFFFFFFF}; + +const VendorId_t VENDOR_UNKNOWN = {}; +} // namespace rtps + +#endif // RTPS_TYPES_H diff --git a/components/rtps_embedded/include/rtps/communication/EsppTransport.h b/components/rtps_embedded/include/rtps/communication/EsppTransport.h new file mode 100644 index 000000000..c56628e5b --- /dev/null +++ b/components/rtps_embedded/include/rtps/communication/EsppTransport.h @@ -0,0 +1,79 @@ +/* +The MIT License +Copyright (c) 2019 Lehrstuhl Informatik 11 - RWTH Aachen University +Modifications Copyright (c) 2026 ATDev +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: +The above copyright notice and this permission notice shall be included in +all copies or substantial portions of the Software. +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +THE SOFTWARE + +This file is part of embeddedRTPS. + +Author: i11 - Embedded Software, RWTH Aachen University +*/ + +#ifndef RTPS_ESPPTRANSPORT_H +#define RTPS_ESPPTRANSPORT_H + +#include "base_component.hpp" +#include "rtps/common/types.h" +#include "rtps/config.h" +#include "udp_socket.hpp" +#include "rtps/communication/PacketInfo.h" + +#include +#include +#include +#include +#include + +namespace rtps { + +class EsppTransport : public espp::BaseComponent { +public: + using RxCallback = ReceiveCallback; + + EsppTransport(RxCallback callback, void *args); + ~EsppTransport() = default; + + bool ensureReceivePort(Ip4Port_t receivePort); + bool joinMultiCastGroup(const Ip4AddressBytes &addr) const; + void sendPacket(PacketInfo &info); + +private: + struct Channel { + Ip4Port_t port{0}; + std::unique_ptr socket{}; + bool in_use{false}; + }; + + Channel *findChannel(Ip4Port_t port); + const Channel *findChannel(Ip4Port_t port) const; + Channel *createChannel(Ip4Port_t receivePort); + bool startReceiver(Channel &channel, Ip4Port_t receivePort); + void onReceive(Ip4Port_t receivePort, std::vector &data, + const espp::Socket::Info &sender) const; + + static std::string ip4ToString(const Ip4AddressBytes &addr); + + RxCallback m_rxCallback{nullptr}; + void *m_callbackArgs{nullptr}; + mutable std::recursive_mutex m_mutex; + std::array m_channels{}; + mutable std::vector m_multicastGroups; +}; + +} // namespace rtps + +#endif // RTPS_ESPPTRANSPORT_H diff --git a/components/rtps_embedded/include/rtps/communication/PacketInfo.h b/components/rtps_embedded/include/rtps/communication/PacketInfo.h new file mode 100644 index 000000000..b679c17fe --- /dev/null +++ b/components/rtps_embedded/include/rtps/communication/PacketInfo.h @@ -0,0 +1,65 @@ +/* +The MIT License +Copyright (c) 2019 Lehrstuhl Informatik 11 - RWTH Aachen University +Modifications Copyright (c) 2026 ATDev +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: +The above copyright notice and this permission notice shall be included in +all copies or substantial portions of the Software. +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +THE SOFTWARE + +This file is part of embeddedRTPS. + +Author: i11 - Embedded Software, RWTH Aachen University +*/ + +// Copyright 2023 Apex.AI, Inc. +// All rights reserved. + +#ifndef RTPS_PACKETINFO_H +#define RTPS_PACKETINFO_H + +#include +#include + +#include "rtps/common/types.h" + +namespace rtps { + +struct PacketInfo { + Ip4Port_t srcPort; // TODO Do we need that? + std::array destAddr = {0, 0, 0, 0}; + Ip4Port_t destPort; + std::vector payload; + + void copyTriviallyCopyable(const PacketInfo &other) { + this->srcPort = other.srcPort; + this->destPort = other.destPort; + this->destAddr = other.destAddr; + } + + PacketInfo() = default; + ~PacketInfo() = default; + + PacketInfo &operator=(const PacketInfo &other) = delete; + + PacketInfo &operator=(PacketInfo &&other) noexcept { + copyTriviallyCopyable(other); + this->payload = std::move(other.payload); + return *this; + } + +}; +} // namespace rtps + +#endif // RTPS_PACKETINFO_H diff --git a/components/rtps_embedded/include/rtps/config.h b/components/rtps_embedded/include/rtps/config.h new file mode 100644 index 000000000..eac533a8c --- /dev/null +++ b/components/rtps_embedded/include/rtps/config.h @@ -0,0 +1,39 @@ +/* +The MIT License +Copyright (c) 2019 Lehrstuhl Informatik 11 - RWTH Aachen University +Modifications Copyright (c) 2026 ATDev +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: +The above copyright notice and this permission notice shall be included in +all copies or substantial portions of the Software. +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +THE SOFTWARE + +This file is part of embeddedRTPS. + +Author: i11 - Embedded Software, RWTH Aachen University +*/ + +#ifndef RTPS_CONFIG_H +#define RTPS_CONFIG_H + +#ifdef RTPS_CONFIG_HEADER +#include RTPS_CONFIG_HEADER +#else +#if defined(ESP_PLATFORM) +#include "rtps/config_esp32.h" +#else +#include "rtps/config_desktop.h" +#endif +#endif + +#endif // RTPS_CONFIG_H diff --git a/components/rtps_embedded/include/rtps/config_desktop.h b/components/rtps_embedded/include/rtps/config_desktop.h new file mode 100644 index 000000000..999b1c959 --- /dev/null +++ b/components/rtps_embedded/include/rtps/config_desktop.h @@ -0,0 +1,105 @@ +/* +The MIT License +Copyright (c) 2019 Lehrstuhl Informatik 11 - RWTH Aachen University +Modifications Copyright (c) 2026 ATDev +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: +The above copyright notice and this permission notice shall be included in +all copies or substantial portions of the Software. +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +THE SOFTWARE + +This file is part of embeddedRTPS. + +Author: i11 - Embedded Software, RWTH Aachen University +*/ + +#ifndef RTPS_CONFIG_DESKTOP_H +#define RTPS_CONFIG_DESKTOP_H + +#include "rtps/common/types.h" + +namespace rtps { + +#define IS_LITTLE_ENDIAN 1 + +namespace Config { +const VendorId_t VENDOR_ID = {13, 37}; +const std::array IP_ADDRESS = { + 192, 168, 4, 1}; // Needs to be set in lwipcfg.h too. +const GuidPrefix_t BASE_GUID_PREFIX{1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 9}; + +const uint8_t DOMAIN_ID = 0; // 230 possible with UDP +const uint8_t MAX_NUM_PARTICIPANTS = 2; +const uint8_t NUM_STATELESS_WRITERS = + MAX_NUM_PARTICIPANTS + 1; // Required + Additional +const uint8_t NUM_STATELESS_READERS = + MAX_NUM_PARTICIPANTS + 1; // Required + Additional +const uint8_t NUM_STATEFUL_READERS = + 4; // 1-4 required per participant depending on what they do and to whom + // they match +const uint8_t NUM_STATEFUL_WRITERS = + 4; // 1-4 required per participant depending on what they do and to whom + // they match +const uint8_t NUM_WRITERS_PER_PARTICIPANT = 4; +const uint8_t NUM_READERS_PER_PARTICIPANT = 4; +const uint8_t NUM_WRITER_PROXIES_PER_READER = 3; +const uint8_t NUM_READER_PROXIES_PER_WRITER = 3; + +const uint8_t MAX_NUM_UNMATCHED_REMOTE_WRITERS = 100; +const uint8_t MAX_NUM_UNMATCHED_REMOTE_READERS = 10; + +const uint8_t MAX_NUM_READER_CALLBACKS = 5; + +const uint8_t HISTORY_SIZE_STATELESS = 2; +const uint8_t HISTORY_SIZE_STATEFUL = 10; + +const uint8_t MAX_TYPENAME_LENGTH = 64; +const uint8_t MAX_TOPICNAME_LENGTH = 64; + +const int HEARTBEAT_STACKSIZE = 1200; // byte +const int THREAD_POOL_WRITER_STACKSIZE = 1100; // byte +const int THREAD_POOL_READER_STACKSIZE = 1600; // byte +const uint16_t SPDP_WRITER_STACKSIZE = 550; // byte + +const uint16_t SF_WRITER_HB_PERIOD_MS = 2000; +const uint16_t SPDP_RESEND_PERIOD_MS = 1000; +const uint8_t SPDP_CYCLECOUNT_HEARTBEAT = + 2; // skip x SPDP rounds before checking liveliness +const uint8_t SPDP_WRITER_PRIO = 3; +const uint8_t SPDP_MAX_NUMBER_FOUND_PARTICIPANTS = 5; +const uint8_t SPDP_MAX_NUM_LOCATORS = 5; +const Duration_t SPDP_DEFAULT_REMOTE_LEASE_DURATION = { + 100, 0}; // Default lease duration for remote participants, usually + // overwritten by remote info +const Duration_t SPDP_MAX_REMOTE_LEASE_DURATION = { + 180, + 0}; // Absolute maximum lease duration, ignoring remote participant info + +const int MAX_NUM_UDP_CONNECTIONS = 10; + +const int THREAD_POOL_NUM_WRITERS = 2; +const int THREAD_POOL_NUM_READERS = 2; +const int THREAD_POOL_WRITER_PRIO = 3; +const int THREAD_POOL_READER_PRIO = 3; +const int THREAD_POOL_WORKLOAD_QUEUE_LENGTH_USERTRAFFIC = 10; +const int THREAD_POOL_WORKLOAD_QUEUE_LENGTH_METATRAFFIC = 10; + +constexpr int OVERALL_HEAP_SIZE = + THREAD_POOL_NUM_WRITERS * THREAD_POOL_WRITER_STACKSIZE + + THREAD_POOL_NUM_READERS * THREAD_POOL_READER_STACKSIZE + + MAX_NUM_PARTICIPANTS * SPDP_WRITER_STACKSIZE + + NUM_STATEFUL_WRITERS * HEARTBEAT_STACKSIZE; +} // namespace Config +} // namespace rtps + +#endif // RTPS_CONFIG_DESKTOP_H diff --git a/components/rtps_embedded/include/rtps/config_esp32.h b/components/rtps_embedded/include/rtps/config_esp32.h new file mode 100644 index 000000000..b20a23a70 --- /dev/null +++ b/components/rtps_embedded/include/rtps/config_esp32.h @@ -0,0 +1,102 @@ +/* +The MIT License +Copyright (c) 2019 Lehrstuhl Informatik 11 - RWTH Aachen University +Modifications Copyright (c) 2026 ATDev +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: +The above copyright notice and this permission notice shall be included in +all copies or substantial portions of the Software. +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +THE SOFTWARE + +This file is part of embeddedRTPS. + +Author: i11 - Embedded Software, RWTH Aachen University +*/ + +#ifndef RTPS_CONFIG_ESP32_H +#define RTPS_CONFIG_ESP32_H + +#include "rtps/common/types.h" + +namespace rtps { + +#define IS_LITTLE_ENDIAN 1 +#define OS_IS_FREERTOS + +namespace Config { +const VendorId_t VENDOR_ID = {13, 37}; +const std::array IP_ADDRESS = { + 192, 168, 4, 1}; // Fallback: must match DHCPS server netif IP. +const GuidPrefix_t BASE_GUID_PREFIX{1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 13}; + +const uint8_t DOMAIN_ID = 0; // 230 possible with UDP +const uint8_t NUM_STATELESS_WRITERS = 5; +const uint8_t NUM_STATELESS_READERS = 5; +const uint8_t NUM_STATEFUL_READERS = 5; +const uint8_t NUM_STATEFUL_WRITERS = 5; +const uint8_t MAX_NUM_PARTICIPANTS = 1; +const uint8_t NUM_WRITERS_PER_PARTICIPANT = 10; +const uint8_t NUM_READERS_PER_PARTICIPANT = 10; +const uint8_t NUM_WRITER_PROXIES_PER_READER = 6; +const uint8_t NUM_READER_PROXIES_PER_WRITER = 6; + +const uint8_t MAX_NUM_UNMATCHED_REMOTE_WRITERS = 50; +const uint8_t MAX_NUM_UNMATCHED_REMOTE_READERS = 50; + +const uint8_t MAX_NUM_READER_CALLBACKS = 5; + +const uint8_t HISTORY_SIZE_STATELESS = 2; +const uint8_t HISTORY_SIZE_STATEFUL = 10; + +const uint8_t MAX_TYPENAME_LENGTH = 64; +const uint8_t MAX_TOPICNAME_LENGTH = 64; + +const int HEARTBEAT_STACKSIZE = 1024*6; // byte +const int THREAD_POOL_WRITER_STACKSIZE = 4096; // byte +const int THREAD_POOL_READER_STACKSIZE = 1024*6; // byte +const uint16_t SPDP_WRITER_STACKSIZE = 4096; // byte + +const uint16_t SF_WRITER_HB_PERIOD_MS = 4000; +const uint16_t SPDP_RESEND_PERIOD_MS = 2000; +const uint8_t SPDP_CYCLECOUNT_HEARTBEAT = + 2; // skip x SPDP rounds before checking liveliness +const uint8_t SPDP_WRITER_PRIO = 5; +const uint8_t SPDP_MAX_NUMBER_FOUND_PARTICIPANTS = 10; +const uint8_t SPDP_MAX_NUM_LOCATORS = 1; +const Duration_t SPDP_DEFAULT_REMOTE_LEASE_DURATION = { + 5, 0}; // Default lease duration for remote participants, usually + // overwritten by remote info +const Duration_t SPDP_MAX_REMOTE_LEASE_DURATION = { + 90, + 0}; // Absolute maximum lease duration, ignoring remote participant info + +const Duration_t SPDP_LEASE_DURATION = {5, 0}; + +const int MAX_NUM_UDP_CONNECTIONS = 10; + +const int THREAD_POOL_NUM_WRITERS = 2; +const int THREAD_POOL_NUM_READERS = 2; +const int THREAD_POOL_WRITER_PRIO = 5; +const int THREAD_POOL_READER_PRIO = 5; +const int THREAD_POOL_WORKLOAD_QUEUE_LENGTH_USERTRAFFIC = 60; +const int THREAD_POOL_WORKLOAD_QUEUE_LENGTH_METATRAFFIC = 60; + +constexpr int OVERALL_HEAP_SIZE = + THREAD_POOL_NUM_WRITERS * THREAD_POOL_WRITER_STACKSIZE + + THREAD_POOL_NUM_READERS * THREAD_POOL_READER_STACKSIZE + + MAX_NUM_PARTICIPANTS * SPDP_WRITER_STACKSIZE + + NUM_STATEFUL_WRITERS * HEARTBEAT_STACKSIZE; +} // namespace Config +} // namespace rtps + +#endif // RTPS_CONFIG_ESP32_H diff --git a/components/rtps_embedded/include/rtps/discovery/BuiltInEndpoints.h b/components/rtps_embedded/include/rtps/discovery/BuiltInEndpoints.h new file mode 100644 index 000000000..922fc01a8 --- /dev/null +++ b/components/rtps_embedded/include/rtps/discovery/BuiltInEndpoints.h @@ -0,0 +1,46 @@ +/* +The MIT License +Copyright (c) 2019 Lehrstuhl Informatik 11 - RWTH Aachen University +Modifications Copyright (c) 2026 ATDev +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: +The above copyright notice and this permission notice shall be included in +all copies or substantial portions of the Software. +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +THE SOFTWARE + +This file is part of embeddedRTPS. + +Author: i11 - Embedded Software, RWTH Aachen University +*/ + +#ifndef RTPS_BUILTINENDPOINTS_H +#define RTPS_BUILTINENDPOINTS_H + +#include "rtps/entities/StatefulReader.h" +#include "rtps/entities/StatelessReader.h" +#include "rtps/entities/StatelessWriter.h" +#include "rtps/entities/Writer.h" + +namespace rtps { + +struct BuiltInEndpoints { + Writer *spdpWriter = nullptr; + Reader *spdpReader = nullptr; + Writer *sedpPubWriter = nullptr; + Reader *sedpPubReader = nullptr; + Writer *sedpSubWriter = nullptr; + Reader *sedpSubReader = nullptr; +}; +} // namespace rtps + +#endif // RTPS_BUILTINENDPOINTS_H diff --git a/components/rtps_embedded/include/rtps/discovery/ParticipantProxyData.h b/components/rtps_embedded/include/rtps/discovery/ParticipantProxyData.h new file mode 100644 index 000000000..95b8cda44 --- /dev/null +++ b/components/rtps_embedded/include/rtps/discovery/ParticipantProxyData.h @@ -0,0 +1,180 @@ +/* +The MIT License +Copyright (c) 2019 Lehrstuhl Informatik 11 - RWTH Aachen University +Modifications Copyright (c) 2026 ATDev +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: +The above copyright notice and this permission notice shall be included in +all copies or substantial portions of the Software. +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +THE SOFTWARE + +This file is part of embeddedRTPS. + +Author: i11 - Embedded Software, RWTH Aachen University +*/ + +#ifndef RTPS_PARTICIPANTPROXYDATA_H +#define RTPS_PARTICIPANTPROXYDATA_H + +#include "base_component.hpp" +#include "rtps/config.h" +#include "rtps/messages/MessageTypes.h" +#include "ucdr/microcdr.h" +#include +#include +#include +#include +#include + +namespace rtps { + +class Participant; +using SMElement::ParameterId; + +using BuiltinEndpointSet_t = uint32_t; + +class ParticipantProxyData : public espp::BaseComponent { +public: + ParticipantProxyData() + : espp::BaseComponent("RtpsParticipantProxy", + espp::Logger::Verbosity::WARN) { + onAliveSignal(); + } + ParticipantProxyData(Guid_t guid); + + ProtocolVersion_t m_protocolVersion = PROTOCOLVERSION; + Guid_t m_guid = Guid_t{GUIDPREFIX_UNKNOWN, ENTITYID_UNKNOWN}; + VendorId_t m_vendorId = VENDOR_UNKNOWN; + bool m_expectsInlineQos = false; + BuiltinEndpointSet_t m_availableBuiltInEndpoints{0}; + std::array + m_metatrafficUnicastLocatorList; + std::array + m_metatrafficMulticastLocatorList; + std::array + m_defaultUnicastLocatorList; + std::array + m_defaultMulticastLocatorList; + Count_t m_manualLivelinessCount{1}; + Duration_t m_leaseDuration = Config::SPDP_DEFAULT_REMOTE_LEASE_DURATION; + std::chrono::time_point + m_lastLivelinessReceivedTimestamp; + void reset(); + + bool readFromUcdrBuffer(ucdrBuffer &buffer, Participant *participant); + + inline bool hasParticipantWriter() const; + inline bool hasParticipantReader() const; + inline bool hasPublicationWriter() const; + inline bool hasPublicationReader() const; + inline bool hasSubscriptionWriter() const; + inline bool hasSubscriptionReader() const; + + inline void onAliveSignal(); + inline bool isAlive() const; + inline uint32_t getAliveSignalAgeInMilliseconds() const; + +private: + bool readLocatorIntoList( + ucdrBuffer &buffer, + std::array &list); + + static const BuiltinEndpointSet_t + DISC_BUILTIN_ENDPOINT_PARTICIPANT_ANNOUNCER = 1 << 0; + static const BuiltinEndpointSet_t DISC_BUILTIN_ENDPOINT_PARTICIPANT_DETECTOR = + 1 << 1; + static const BuiltinEndpointSet_t + DISC_BUILTIN_ENDPOINT_PUBLICATION_ANNOUNCER = 1 << 2; + static const BuiltinEndpointSet_t DISC_BUILTIN_ENDPOINT_PUBLICATION_DETECTOR = + 1 << 3; + static const BuiltinEndpointSet_t + DISC_BUILTIN_ENDPOINT_SUBSCRIPTION_ANNOUNCER = 1 << 4; + static const BuiltinEndpointSet_t + DISC_BUILTIN_ENDPOINT_SUBSCRIPTION_DETECTOR = 1 << 5; + static const BuiltinEndpointSet_t + DISC_BUILTIN_ENDPOINT_PARTICIPANT_PROXY_ANNOUNCER = 1 << 6; + static const BuiltinEndpointSet_t + DISC_BUILTIN_ENDPOINT_PARTICIPANT_PROXY_DETECTOR = 1 << 7; + static const BuiltinEndpointSet_t + DISC_BUILTIN_ENDPOINT_PARTICIPANT_STATE_ANNOUNCER = 1 << 8; + static const BuiltinEndpointSet_t + DISC_BUILTIN_ENDPOINT_PARTICIPANT_STATE_DETECTOR = 1 << 9; + static const BuiltinEndpointSet_t + BUILTIN_ENDPOINT_PARTICIPANT_MESSAGE_DATA_WRITER = 1 << 10; + static const BuiltinEndpointSet_t + BUILTIN_ENDPOINT_PARTICIPANT_MESSAGE_DATA_READER = 1 << 11; +}; + +// Needs to be in header because they are marked with inline +bool ParticipantProxyData::hasParticipantWriter() const { + return (m_availableBuiltInEndpoints & + DISC_BUILTIN_ENDPOINT_PARTICIPANT_ANNOUNCER) == 1; +} + +bool ParticipantProxyData::hasParticipantReader() const { + return (m_availableBuiltInEndpoints & + DISC_BUILTIN_ENDPOINT_PARTICIPANT_DETECTOR) != 0; +} + +bool ParticipantProxyData::hasPublicationWriter() const { + return (m_availableBuiltInEndpoints & + DISC_BUILTIN_ENDPOINT_PUBLICATION_ANNOUNCER) != 0; +} + +bool ParticipantProxyData::hasPublicationReader() const { + return (m_availableBuiltInEndpoints & + DISC_BUILTIN_ENDPOINT_PUBLICATION_DETECTOR) != 0; +} + +bool ParticipantProxyData::hasSubscriptionWriter() const { + return (m_availableBuiltInEndpoints & + DISC_BUILTIN_ENDPOINT_SUBSCRIPTION_ANNOUNCER) != 0; +} + +bool ParticipantProxyData::hasSubscriptionReader() const { + return (m_availableBuiltInEndpoints & + DISC_BUILTIN_ENDPOINT_SUBSCRIPTION_DETECTOR) != 0; +} + +void ParticipantProxyData::onAliveSignal() { + m_lastLivelinessReceivedTimestamp = std::chrono::steady_clock::now(); +} + +uint32_t ParticipantProxyData::getAliveSignalAgeInMilliseconds() const { + auto now = std::chrono::steady_clock::now(); + auto duration = std::chrono::duration_cast( + now - m_lastLivelinessReceivedTimestamp); + return static_cast(duration.count()); +} + +/* + * Returns true if last heartbeat within lease duration, else false + */ +bool ParticipantProxyData::isAlive() const { + uint32_t lease_in_ms = + m_leaseDuration.seconds * 1000 + m_leaseDuration.fraction * 1e-6; + + uint32_t max_lease_in_ms = + Config::SPDP_MAX_REMOTE_LEASE_DURATION.seconds * 1000 + + Config::SPDP_MAX_REMOTE_LEASE_DURATION.fraction * 1e-6; + + auto heatbeat_age_in_ms = getAliveSignalAgeInMilliseconds(); + + if (heatbeat_age_in_ms > std::min(lease_in_ms, max_lease_in_ms)) { + return false; + } + return true; +} + +} // namespace rtps +#endif // RTPS_PARTICIPANTPROXYDATA_H diff --git a/components/rtps_embedded/include/rtps/discovery/SEDPAgent.h b/components/rtps_embedded/include/rtps/discovery/SEDPAgent.h new file mode 100644 index 000000000..d1f6b711f --- /dev/null +++ b/components/rtps_embedded/include/rtps/discovery/SEDPAgent.h @@ -0,0 +1,127 @@ +/* +The MIT License +Copyright (c) 2019 Lehrstuhl Informatik 11 - RWTH Aachen University +Modifications Copyright (c) 2026 ATDev +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: +The above copyright notice and this permission notice shall be included in +all copies or substantial portions of the Software. +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +THE SOFTWARE + +This file is part of embeddedRTPS. + +Author: i11 - Embedded Software, RWTH Aachen University +*/ + +#ifndef RTPS_SEDPAGENT_H +#define RTPS_SEDPAGENT_H + +#include "base_component.hpp" +#include "rtps/discovery/BuiltInEndpoints.h" +#include "rtps/config.h" +#include "rtps/discovery/TopicData.h" + +#include +#include + +namespace rtps { + +class Participant; +class ReaderCacheChange; +class Writer; +class Reader; + +class SEDPAgent : public espp::BaseComponent { +public: + SEDPAgent(); + void init(Participant &part, const BuiltInEndpoints &endpoints); + bool addWriter(Writer &writer); + bool addReader(Reader &reader); + bool deleteReader(Reader *reader); + bool deleteWriter(Writer *reader); + + void registerOnNewPublisherMatchedCallback(void (*callback)(void *arg), + void *args); + void registerOnNewSubscriberMatchedCallback(void (*callback)(void *arg), + void *args); + void removeUnmatchedEntitiesOfParticipant(const GuidPrefix_t &guidPrefix); + void removeUnmatchedEntity(const Guid_t &guid); + + uint32_t getNumRemoteUnmatchedReaders(); + uint32_t getNumRemoteUnmatchedWriters(); + +protected: // For testing purposes + void handlePublisherReaderMessage(const TopicData &writerData, + const ReaderCacheChange &change); + void handleSubscriptionReaderMessage(const TopicData &writerData, + const ReaderCacheChange &change); + +private: + Participant *m_part = nullptr; + std::recursive_mutex m_mutex; + uint8_t m_buffer[600]; // TODO check size, currently changed from 300 to 600 + // (FastDDS gives too many options) + BuiltInEndpoints m_endpoints; + /* + * If we add readers later on, remote participants will not send matching + * writer proxies again (and vice versa). This is done only once during + * discovery. Therefore, we need to keep track of remote endpoints. Topic and + * type are represented as hash values to save memory. + */ + MemoryPool + m_unmatchedRemoteWriters; + size_t m_numUnmatchedRemoteWriters = 0; + MemoryPool + m_unmatchedRemoteReaders; + size_t m_numMatchedRemoteReaders = 0; + + void tryMatchUnmatchedEndpoints(); + void addUnmatchedRemoteWriter(const TopicData &writerData); + void addUnmatchedRemoteReader(const TopicData &readerData); + void addUnmatchedRemoteWriter(const TopicDataCompressed &writerData); + void addUnmatchedRemoteReader(const TopicDataCompressed &readerData); + + void handleRemoteEndpointDeletion(const TopicData &topic, + const ReaderCacheChange &change); + + void (*mfp_onNewPublisherCallback)(void *arg) = nullptr; + void *m_onNewPublisherArgs = nullptr; + void (*mfp_onNewSubscriberCallback)(void *arg) = nullptr; + void *m_onNewSubscriberArgs = nullptr; + + static void jumppadPublisherReader(void *callee, + const ReaderCacheChange &cacheChange); + static void jumppadSubscriptionReader(void *callee, + const ReaderCacheChange &cacheChange); + + static void jumppadTakeProxyOfDisposedReader(const Reader *reader, + const WriterProxy &proxy, + void *arg); + static void jumppadTakeProxyOfDisposedWriter(const Writer *writer, + const ReaderProxy &proxy, + void *arg); + + void handlePublisherReaderMessage(const ReaderCacheChange &change); + void handleSubscriptionReaderMessage(const ReaderCacheChange &change); + + template bool deleteEndpoint(A *endpoint, Writer *sedp_endpoint); + + template + bool announceEndpointDeletion(A *local_endpoint, Writer *sedp_endpoint); + + template + bool disposeEndpointInSEDPHistory(A *local_endpoint, Writer *sedp_writer); +}; +} // namespace rtps + +#endif // RTPS_SEDPAGENT_H diff --git a/components/rtps_embedded/include/rtps/discovery/SPDPAgent.h b/components/rtps_embedded/include/rtps/discovery/SPDPAgent.h new file mode 100644 index 000000000..501df7b44 --- /dev/null +++ b/components/rtps_embedded/include/rtps/discovery/SPDPAgent.h @@ -0,0 +1,89 @@ +/* +The MIT License +Copyright (c) 2019 Lehrstuhl Informatik 11 - RWTH Aachen University +Modifications Copyright (c) 2026 ATDev +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: +The above copyright notice and this permission notice shall be included in +all copies or substantial portions of the Software. +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +THE SOFTWARE + +This file is part of embeddedRTPS. + +Author: i11 - Embedded Software, RWTH Aachen University +*/ + +#ifndef RTPS_SPDP_H +#define RTPS_SPDP_H + +#include "base_component.hpp" +#include "rtps/common/types.h" +#include "rtps/config.h" +#include "rtps/discovery/BuiltInEndpoints.h" +#include "rtps/discovery/ParticipantProxyData.h" +#include "rtps/utils/Log.h" +#include "task.hpp" +#include "ucdr/microcdr.h" + +#include +#include + +#if SPDP_VERBOSE && RTPS_GLOBAL_VERBOSE +#include "rtps/utils/printutils.h" +#define SPDP_LOG(...) logger_.warn(__VA_ARGS__) +#else +#define SPDP_LOG(...) do { } while (0) +#endif + +namespace rtps { +class Participant; +class Writer; +class Reader; +class ReaderCacheChange; + +class SPDPAgent : public espp::BaseComponent { +public: + SPDPAgent(); + void init(Participant &participant, BuiltInEndpoints &endpoints); + void start(); + void stop(); + std::recursive_mutex m_mutex; + +private: + Participant *mp_participant = nullptr; + BuiltInEndpoints m_buildInEndpoints; + std::unique_ptr m_broadcastTask; + bool m_running = false; + std::array m_outputBuffer{}; // TODO check required size + std::array m_inputBuffer{}; + ParticipantProxyData m_proxyDataBuffer{}; + ucdrBuffer m_microbuffer{}; + uint8_t m_cycleHB = 0; + + bool initialized = false; + static void receiveCallback(void *callee, + const ReaderCacheChange &cacheChange); + void handleSPDPPackage(const ReaderCacheChange &cacheChange); + void configureEndianessAndOptions(ucdrBuffer &buffer); + void processProxyData(); + bool addProxiesForBuiltInEndpoints(); + + void addInlineQos(); + void addParticipantParameters(); + void endCurrentList(); + + void runBroadcast(); +}; +} // namespace rtps + +#endif // RTPS_SPDP_H diff --git a/components/rtps_embedded/include/rtps/discovery/TopicData.h b/components/rtps_embedded/include/rtps/discovery/TopicData.h new file mode 100644 index 000000000..8071f108d --- /dev/null +++ b/components/rtps_embedded/include/rtps/discovery/TopicData.h @@ -0,0 +1,110 @@ +/* +The MIT License +Copyright (c) 2019 Lehrstuhl Informatik 11 - RWTH Aachen University +Modifications Copyright (c) 2026 ATDev +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: +The above copyright notice and this permission notice shall be included in +all copies or substantial portions of the Software. +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +THE SOFTWARE + +This file is part of embeddedRTPS. + +Author: i11 - Embedded Software, RWTH Aachen University +*/ + +#ifndef RTPS_DISCOVEREDWRITERDATA_H +#define RTPS_DISCOVEREDWRITERDATA_H + +#define SUPPRESS_UNICAST 0 + +#include "rtps/config.h" +#include "rtps/utils/hash.h" +#include "ucdr/microcdr.h" +#include +#include + +namespace rtps { + +struct BuiltInTopicKey { + std::array value; +}; + +struct TopicData { + Guid_t endpointGuid; + char typeName[Config::MAX_TYPENAME_LENGTH]; + char topicName[Config::MAX_TOPICNAME_LENGTH]; + ReliabilityKind_t reliabilityKind; + DurabilityKind_t durabilityKind; + FullLengthLocator unicastLocator; + FullLengthLocator multicastLocator; + + uint8_t statusInfo; + bool statusInfoValid; + // Use Case: Remotes communicates id of deleted endpoint through key_hash + // parameter + EntityId_t entityIdFromKeyHash; + bool entityIdFromKeyHashValid; + + TopicData() + : endpointGuid(GUID_UNKNOWN), typeName{'\0'}, topicName{'\0'}, + reliabilityKind(ReliabilityKind_t::BEST_EFFORT), + durabilityKind(DurabilityKind_t::VOLATILE) { + rtps::FullLengthLocator someLocator = + rtps::FullLengthLocator::createUDPv4Locator( + 192, 168, 0, 42, rtps::getUserUnicastPort(0)); + unicastLocator = someLocator; + multicastLocator = FullLengthLocator(); + }; + + TopicData(Guid_t guid, ReliabilityKind_t reliability, FullLengthLocator loc) + : endpointGuid(guid), typeName{'\0'}, topicName{'\0'}, + reliabilityKind(reliability), + durabilityKind(DurabilityKind_t::VOLATILE), unicastLocator(loc) {} + + + bool matchesTopicOf(const TopicData &other); + + bool readFromUcdrBuffer(ucdrBuffer &buffer); + bool serializeIntoUcdrBuffer(ucdrBuffer &buffer) const; + + bool isDisposedFlagSet() const; + bool isUnregisteredFlagSet() const; +}; + +struct TopicDataCompressed { + Guid_t endpointGuid; + std::size_t topicHash; + std::size_t typeHash; + bool is_reliable; + LocatorIPv4 unicastLocator; + LocatorIPv4 multicastLocator; + + TopicDataCompressed() = default; + TopicDataCompressed(const TopicData &topic_data) { + endpointGuid = topic_data.endpointGuid; + topicHash = + hashCharArray(topic_data.topicName, Config::MAX_TOPICNAME_LENGTH); + typeHash = hashCharArray(topic_data.typeName, Config::MAX_TYPENAME_LENGTH); + is_reliable = (topic_data.reliabilityKind == ReliabilityKind_t::RELIABLE) + ? true + : false; + unicastLocator = topic_data.unicastLocator; + multicastLocator = topic_data.multicastLocator; + } + + bool matchesTopicOf(const TopicData &topic_data) const; +}; +} // namespace rtps + +#endif // RTPS_DISCOVEREDWRITERDATA_H diff --git a/components/rtps_embedded/include/rtps/entities/Domain.h b/components/rtps_embedded/include/rtps/entities/Domain.h new file mode 100644 index 000000000..fcfdd45fd --- /dev/null +++ b/components/rtps_embedded/include/rtps/entities/Domain.h @@ -0,0 +1,109 @@ +/* +The MIT License +Copyright (c) 2019 Lehrstuhl Informatik 11 - RWTH Aachen University +Modifications Copyright (c) 2026 ATDev +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: +The above copyright notice and this permission notice shall be included in +all copies or substantial portions of the Software. +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +THE SOFTWARE + +This file is part of embeddedRTPS. + +Author: i11 - Embedded Software, RWTH Aachen University +*/ + +#ifndef RTPS_DOMAIN_H +#define RTPS_DOMAIN_H + +#include "base_component.hpp" +#include "rtps/ThreadPool.h" +#include "rtps/communication/EsppTransport.h" +#include "rtps/config.h" +#include "rtps/entities/Participant.h" +#include "rtps/entities/StatefulReader.h" +#include "rtps/entities/StatefulWriter.h" +#include "rtps/entities/StatelessReader.h" +#include "rtps/entities/StatelessWriter.h" +#include "rtps/common/types.h" +#include +#include + +namespace rtps { +class Domain : public espp::BaseComponent { +public: + explicit Domain(const Ip4AddressBytes &localIpAddress); + Domain(EsppTransport &transport, + const Ip4AddressBytes &localIpAddress); + ~Domain(); + + bool completeInit(); + void stop(); + + Participant *createParticipant(); + Writer *createWriter(Participant &part, const char *topicName, + const char *typeName, bool reliable, + bool enforceUnicast = false); + Reader *createReader(Participant &part, const char *topicName, + const char *typeName, bool reliable, + Ip4AddressBytes mcastaddress = + {0, 0, 0, 0}); + + Writer *writerExists(Participant &part, const char *topicName, + const char *typeName, bool reliable); + Reader *readerExists(Participant &part, const char *topicName, + const char *typeName, bool reliable); + + bool deleteWriter(Participant &part, Writer *writer); + bool deleteReader(Participant &part, Reader *reader); + + void printInfo(); + +private: + friend class SizeInspector; + ThreadPool m_threadPool; + using DefaultTransport = EsppTransport; + DefaultTransport m_defaultTransport; + EsppTransport *m_transport = nullptr; + std::array m_participants; + Ip4AddressBytes m_localIpAddress{{0, 0, 0, 0}}; + const uint8_t PARTICIPANT_START_ID = 0; + ParticipantId_t m_nextParticipantId = PARTICIPANT_START_ID; + + std::array m_statelessWriters; + std::array m_statelessReaders; + std::array m_statefulReaders; + std::array m_statefulWriters; + template B *getNextUnusedEndpoint(A &a) { + for (unsigned int i = 0; i < a.size(); i++) { + if (!a[i].isInitialized()) { + return &(a[i]); + } + } + return nullptr; + } + + bool m_initComplete = false; + std::recursive_mutex m_mutex; + + void receiveCallback(const PacketInfo &packet); + GuidPrefix_t generateGuidPrefix(ParticipantId_t id) const; + void createBuiltinWritersAndReaders(Participant &part); + void initializeTransport(); + void registerPort(const Participant &part); + void registerMulticastPort(FullLengthLocator mcastLocator); + static void receiveJumppad(void *callee, const PacketInfo &packet); +}; +} // namespace rtps + +#endif // RTPS_DOMAIN_H diff --git a/components/rtps_embedded/include/rtps/entities/Participant.h b/components/rtps_embedded/include/rtps/entities/Participant.h new file mode 100644 index 000000000..8cc36a1b0 --- /dev/null +++ b/components/rtps_embedded/include/rtps/entities/Participant.h @@ -0,0 +1,136 @@ +/* +The MIT License +Copyright (c) 2019 Lehrstuhl Informatik 11 - RWTH Aachen University +Modifications Copyright (c) 2026 ATDev +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: +The above copyright notice and this permission notice shall be included in +all copies or substantial portions of the Software. +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +THE SOFTWARE + +This file is part of embeddedRTPS. + +Author: i11 - Embedded Software, RWTH Aachen University +*/ + +#ifndef RTPS_PARTICIPANT_H +#define RTPS_PARTICIPANT_H + +#include "base_component.hpp" +#include "rtps/common/types.h" +#include "rtps/config.h" +#include "rtps/discovery/SEDPAgent.h" +#include "rtps/discovery/SPDPAgent.h" +#include "rtps/messages/MessageReceiver.h" +#include "rtps/common/types.h" + +#include +#include + +namespace rtps { + +class Writer; +class Reader; + +class Participant : public espp::BaseComponent { +public: + GuidPrefix_t m_guidPrefix; + ParticipantId_t m_participantId; + Ip4AddressBytes m_localIpAddress{{0, 0, 0, 0}}; + + Participant(); + explicit Participant(const GuidPrefix_t &guidPrefix, + ParticipantId_t participantId); + + // Not allowed because the message receiver contains a pointer to the + // participant + Participant(const Participant &) = delete; + Participant(Participant &&) = delete; + Participant &operator=(const Participant &) = delete; + Participant &operator=(Participant &&) = delete; + + ~Participant(); + bool isValid(); + + void reuse(const GuidPrefix_t &guidPrefix, ParticipantId_t participantId); + void reuse(const GuidPrefix_t &guidPrefix, ParticipantId_t participantId, + const Ip4AddressBytes &localIpAddress); + + std::array getNextUserEntityKey(); + + // Actually the only two function that should be used by the user + bool registerOnNewPublisherMatchedCallback(void (*callback)(void *arg), + void *args); + bool registerOnNewSubscriberMatchedCallback(void (*callback)(void *arg), + void *args); + + //! Not-thread-safe function to add a writer + Writer *addWriter(Writer *writer); + bool isWritersFull(); + bool deleteWriter(Writer *writer); + + //! Not-thread-safe function to add a reader + Reader *addReader(Reader *reader); + bool isReadersFull(); + bool deleteReader(Reader *reader); + + //! (Probably) Thread safe if writers cannot be removed + Writer *getWriter(EntityId_t id); + Writer *getMatchingWriter(const TopicData &topicData); + Writer *getMatchingWriter(const TopicDataCompressed &topicData); + + //! (Probably) Thread safe if readers cannot be removed + Reader *getReader(EntityId_t id); + Reader *getReaderByWriterId(const Guid_t &guid); + Reader *getMatchingReader(const TopicData &topicData); + Reader *getMatchingReader(const TopicDataCompressed &topicData); + + bool addNewRemoteParticipant(const ParticipantProxyData &remotePart); + bool removeRemoteParticipant(const GuidPrefix_t &prefix); + void removeAllProxiesOfParticipant(const GuidPrefix_t &prefix); + void removeProxyFromAllEndpoints(const Guid_t &guid); + + const ParticipantProxyData *findRemoteParticipant(const GuidPrefix_t &prefix); + void refreshRemoteParticipantLiveliness(const GuidPrefix_t &prefix); + uint32_t getRemoteParticipantCount(); + MessageReceiver *getMessageReceiver(); + bool checkAndResetHeartbeats(); + + bool hasReaderWithMulticastLocator(const std::array &address); + + void addBuiltInEndpoints(BuiltInEndpoints &endpoints); + void newMessage(const uint8_t *data, DataSize_t size); + + SPDPAgent &getSPDPAgent(); + void printInfo(); + +private: + friend class SizeInspector; + MessageReceiver m_receiver; + bool m_hasBuilInEndpoints = false; + std::array m_nextUserEntityId{{0, 0, 1}}; + std::array m_writers = { + nullptr}; + std::array m_readers = { + nullptr}; + + std::recursive_mutex m_mutex; + MemoryPool + m_remoteParticipants; + + SPDPAgent m_spdpAgent; + SEDPAgent m_sedpAgent; +}; +} // namespace rtps + +#endif // RTPS_PARTICIPANT_H diff --git a/components/rtps_embedded/include/rtps/entities/Reader.h b/components/rtps_embedded/include/rtps/entities/Reader.h new file mode 100644 index 000000000..33bd4d603 --- /dev/null +++ b/components/rtps_embedded/include/rtps/entities/Reader.h @@ -0,0 +1,148 @@ +/* +The MIT License +Copyright (c) 2019 Lehrstuhl Informatik 11 - RWTH Aachen University +Modifications Copyright (c) 2026 ATDev +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: +The above copyright notice and this permission notice shall be included in +all copies or substantial portions of the Software. +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +THE SOFTWARE + +This file is part of embeddedRTPS. + +Author: i11 - Embedded Software, RWTH Aachen University +*/ + +#ifndef RTPS_READER_H +#define RTPS_READER_H + +#include "base_component.hpp" +#include "rtps/common/types.h" +#include "rtps/config.h" +#include "rtps/discovery/TopicData.h" +#include "rtps/entities/WriterProxy.h" +#include "rtps/storages/MemoryPool.h" +#include +#include + +namespace rtps { + +struct SubmessageHeartbeat; +struct SubmessageGap; + +class ReaderCacheChange { +private: + const uint8_t *data; + +public: + const ChangeKind_t kind; + const DataSize_t size; + const Guid_t writerGuid; + const SequenceNumber_t sn; + + ReaderCacheChange(ChangeKind_t kind, Guid_t &writerGuid, SequenceNumber_t sn, + const uint8_t *data, DataSize_t size) + : data(data), kind(kind), size(size), writerGuid(writerGuid), sn(sn){}; + + ~ReaderCacheChange() = + default; // No need to free data. It's not owned by this object + // Not allowed because this class doesn't own the ptr and the user isn't + // allowed to use it outside the Scope of the callback + ReaderCacheChange(const ReaderCacheChange &other) = delete; + ReaderCacheChange(ReaderCacheChange &&other) = delete; + ReaderCacheChange &operator=(const ReaderCacheChange &other) = delete; + ReaderCacheChange &operator=(ReaderCacheChange &&other) = delete; + + bool copyInto(uint8_t *buffer, DataSize_t destSize) const { + if (destSize < size) { + return false; + } else { + memcpy(buffer, data, size); + return true; + } + } + + const uint8_t *getData() const { return data; } + + DataSize_t getDataSize() const { return size; } +}; + +typedef void (*ddsReaderCallback_fp)(void *callee, + const ReaderCacheChange &cacheChange); + +class Reader : public espp::BaseComponent { +public: + using callbackFunction_t = void (*)(void *, const ReaderCacheChange &); + using callbackIdentifier_t = uint32_t; + + TopicData m_attributes; + virtual void newChange(const ReaderCacheChange &cacheChange) = 0; + virtual callbackIdentifier_t registerCallback(callbackFunction_t cb, + void *arg); + virtual bool removeCallback(callbackIdentifier_t identifier); + uint8_t getNumCallbacks(); + + virtual bool onNewHeartbeat(const SubmessageHeartbeat &msg, + const GuidPrefix_t &remotePrefix) = 0; + virtual bool onNewGapMessage(const SubmessageGap &msg, + const GuidPrefix_t &remotePrefix) = 0; + virtual bool addNewMatchedWriter(const WriterProxy &newProxy) = 0; + virtual bool removeProxy(const Guid_t &guid); + virtual void removeAllProxiesOfParticipant(const GuidPrefix_t &guidPrefix); + bool isInitialized() { return m_is_initialized_; } + virtual void reset(); + bool isProxy(const Guid_t &guid); + WriterProxy *getProxy(Guid_t guid); + uint32_t getProxiesCount(); + + void setSEDPSequenceNumber(const SequenceNumber_t &sn); + const SequenceNumber_t &getSEDPSequenceNumber(); + + using dumpProxyCallback = void (*)(const Reader *reader, const WriterProxy &, + void *arg); + + int dumpAllProxies(dumpProxyCallback target, void *arg); + + virtual bool sendPreemptiveAckNack(const WriterProxy &writer); + +protected: + void executeCallbacks(const ReaderCacheChange &cacheChange); + bool initMutex(); + + SequenceNumber_t m_sedp_sequence_number; + + bool m_is_initialized_ = false; + Reader(); + virtual ~Reader() = default; + MemoryPool m_proxies; + + callbackIdentifier_t m_callback_identifier = 1; + + uint8_t m_callback_count = 0; + using callbackElement_t = struct { + callbackFunction_t function; + void *arg; + callbackIdentifier_t identifier; + }; + + std::array m_callbacks; + + // Guards manipulation of the proxies array + std::recursive_mutex m_proxies_mutex; + + // Guards manipulation of callback array + std::recursive_mutex m_callback_mutex; +}; +} // namespace rtps + +#endif // RTPS_READER_H diff --git a/components/rtps_embedded/include/rtps/entities/ReaderProxy.h b/components/rtps_embedded/include/rtps/entities/ReaderProxy.h new file mode 100644 index 000000000..8a49de732 --- /dev/null +++ b/components/rtps_embedded/include/rtps/entities/ReaderProxy.h @@ -0,0 +1,59 @@ +/* +The MIT License +Copyright (c) 2019 Lehrstuhl Informatik 11 - RWTH Aachen University +Modifications Copyright (c) 2026 ATDev +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: +The above copyright notice and this permission notice shall be included in +all copies or substantial portions of the Software. +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +THE SOFTWARE + +This file is part of embeddedRTPS. + +Author: i11 - Embedded Software, RWTH Aachen University +*/ + +#ifndef RTPS_READERPROXY_H +#define RTPS_READERPROXY_H + +#include "rtps/common/types.h" +#include "rtps/discovery/ParticipantProxyData.h" + +namespace rtps { +struct ReaderProxy { + Guid_t remoteReaderGuid; + Count_t ackNackCount = {0}; + LocatorIPv4 remoteLocator; + bool is_reliable = false; + LocatorIPv4 remoteMulticastLocator; + bool useMulticast = false; + bool suppressUnicast = false; + bool unknown_eid = false; + bool finalFlag = false; + SequenceNumber_t lastAckNackSequenceNumber = {0, 1}; + + ReaderProxy() + : remoteReaderGuid({GUIDPREFIX_UNKNOWN, ENTITYID_UNKNOWN}), + ackNackCount{0}, remoteLocator(LocatorIPv4()), finalFlag(false){}; + ReaderProxy(const Guid_t &guid, const LocatorIPv4 &loc, bool reliable) + : remoteReaderGuid(guid), ackNackCount{0}, remoteLocator(loc), + is_reliable(reliable), finalFlag(false){}; + ReaderProxy(const Guid_t &guid, const LocatorIPv4 &loc, + const LocatorIPv4 &mcastloc, bool reliable) + : remoteReaderGuid(guid), ackNackCount{0}, remoteLocator(loc), + is_reliable(reliable), remoteMulticastLocator(mcastloc), finalFlag(false){}; +}; + +} // namespace rtps + +#endif // RTPS_READERPROXY_H diff --git a/components/rtps_embedded/include/rtps/entities/StatefulReader.h b/components/rtps_embedded/include/rtps/entities/StatefulReader.h new file mode 100644 index 000000000..4edbac8e3 --- /dev/null +++ b/components/rtps_embedded/include/rtps/entities/StatefulReader.h @@ -0,0 +1,64 @@ +/* +The MIT License +Copyright (c) 2019 Lehrstuhl Informatik 11 - RWTH Aachen University +Modifications Copyright (c) 2026 ATDev +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: +The above copyright notice and this permission notice shall be included in +all copies or substantial portions of the Software. +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +THE SOFTWARE + +This file is part of embeddedRTPS. + +Author: i11 - Embedded Software, RWTH Aachen University +*/ + +#ifndef RTPS_STATEFULREADER_H +#define RTPS_STATEFULREADER_H + +#include "rtps/communication/PacketInfo.h" +#include "rtps/config.h" +#include "rtps/entities/Reader.h" +#include "rtps/common/types.h" +#include "rtps/entities/WriterProxy.h" +#include "rtps/storages/MemoryPool.h" + +namespace rtps { +class EsppTransport; +struct SubmessageHeartbeat; + +template class StatefulReaderT final : public Reader { +public: + ~StatefulReaderT() override; + bool init(const TopicData &attributes, NetworkDriver &driver); + void newChange(const ReaderCacheChange &cacheChange) override; + bool addNewMatchedWriter(const WriterProxy &newProxy) override; + bool onNewHeartbeat(const SubmessageHeartbeat &msg, + const GuidPrefix_t &remotePrefix) override; + bool onNewGapMessage(const SubmessageGap &msg, + const GuidPrefix_t &remotePrefix) override; + + bool sendPreemptiveAckNack(const WriterProxy &writer) override; + +private: + Ip4Port_t m_srcPort; // TODO intended for reuse but buffer not used as such + NetworkDriver *m_transport; +}; + +using StatefulReader = StatefulReaderT; + +} // namespace rtps + +#include "StatefulReader.tpp" + +#endif // RTPS_STATEFULREADER_H diff --git a/components/rtps_embedded/include/rtps/entities/StatefulReader.tpp b/components/rtps_embedded/include/rtps/entities/StatefulReader.tpp new file mode 100644 index 000000000..3e46dc06c --- /dev/null +++ b/components/rtps_embedded/include/rtps/entities/StatefulReader.tpp @@ -0,0 +1,301 @@ +/* +The MIT License +Copyright (c) 2019 Lehrstuhl Informatik 11 - RWTH Aachen University +Modifications Copyright (c) 2026 ATDev +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: +The above copyright notice and this permission notice shall be included in +all copies or substantial portions of the Software. +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +THE SOFTWARE + +This file is part of embeddedRTPS. + +Author: i11 - Embedded Software, RWTH Aachen University +*/ + +#include "rtps/entities/StatefulReader.h" +#include "rtps/messages/MessageFactory.h" +#include "rtps/storages/PayloadBuffer.h" +#include "rtps/utils/Diagnostics.h" +#include "rtps/utils/Log.h" +#include + +#if SFR_VERBOSE && RTPS_GLOBAL_VERBOSE +#include "rtps/utils/printutils.h" +#define SFR_LOG(...) logger_.warn(__VA_ARGS__) +#else +#define SFR_LOG(...) do { } while (0) +#endif + +using rtps::StatefulReaderT; +using rtps::GuidPrefix_t; +using rtps::Guid_t; +using rtps::PacketInfo; +using rtps::ReaderCacheChange; +using rtps::SequenceNumberSet; +using rtps::SequenceNumber_t; +using rtps::SubmessageGap; +using rtps::SubmessageHeartbeat; +using rtps::TopicData; +using rtps::WriterProxy; + +template +StatefulReaderT::~StatefulReaderT() {} + +template +bool StatefulReaderT::init(const TopicData &attributes, + NetworkDriver &driver) { + if (!initMutex()) { + return false; + } + + m_proxies.clear(); + m_attributes = attributes; + m_transport = &driver; + m_srcPort = attributes.unicastLocator.port; + m_is_initialized_ = true; + return true; +} + +template +void StatefulReaderT::newChange( + const ReaderCacheChange &cacheChange) { + if (m_callback_count == 0 || !m_is_initialized_) { + return; + } + std::lock_guard lock(m_proxies_mutex); + for (auto &proxy : m_proxies) { + if (proxy.remoteWriterGuid == cacheChange.writerGuid) { + if (proxy.expectedSN == cacheChange.sn) { + SFR_LOG("Delivering SN {}.{} | GUID {} {} {} {}", + (int)cacheChange.sn.high, (int)cacheChange.sn.low, + cacheChange.writerGuid.prefix.id[0], + cacheChange.writerGuid.prefix.id[1], + cacheChange.writerGuid.prefix.id[2], + cacheChange.writerGuid.prefix.id[3]); + executeCallbacks(cacheChange); + ++proxy.expectedSN; + SFR_LOG("Done processing SN {}.{}", (int)cacheChange.sn.high, + (int)cacheChange.sn.low); + return; + } else { + Diagnostics::StatefulReader::sfr_unexpected_sn++; + SFR_LOG( + "Unexpected SN {}.{} != {}.{}, dropping! GUID {} {} {} {}", + (int)proxy.expectedSN.high, (int)proxy.expectedSN.low, + (int)cacheChange.sn.high, (int)cacheChange.sn.low, + cacheChange.writerGuid.prefix.id[0], + cacheChange.writerGuid.prefix.id[1], + cacheChange.writerGuid.prefix.id[2], + cacheChange.writerGuid.prefix.id[3]); + } + } + } +} + +template +bool StatefulReaderT::addNewMatchedWriter( + const WriterProxy &newProxy) { +#if SFR_VERBOSE && RTPS_GLOBAL_VERBOSE + SFR_LOG("New writer added"); +#endif + return m_proxies.add(newProxy); +} + +template +bool StatefulReaderT::onNewGapMessage( + const SubmessageGap &msg, const GuidPrefix_t &remotePrefix) { + std::lock_guard lock(m_proxies_mutex); + if (!m_is_initialized_) { + return false; + } + SFR_LOG("Processing gap message {}.{} {}.{}", (int)msg.gapStart.high, + (unsigned int)msg.gapStart.low, (int)msg.gapList.base.high, + (unsigned int)msg.gapList.base.low); + + Guid_t writerProxyGuid; + writerProxyGuid.prefix = remotePrefix; + writerProxyGuid.entityId = msg.writerId; + WriterProxy *writer = getProxy(writerProxyGuid); + + if (writer == nullptr) { + +#if SFR_VERBOSE && RTPS_GLOBAL_VERBOSE + SFR_LOG("Ignore GAP. Couldn't find a matching writer"); +#endif + return false; + } + + // Case 1: We are still waiting for messages before gapStart + if (writer->expectedSN < msg.gapStart) { + PacketInfo info; + info.srcPort = m_srcPort; + info.destAddr = writer->remoteLocator.getIp4AddressBytes(); + info.destPort = writer->remoteLocator.port; + PayloadBuffer payload; + rtps::MessageFactory::addHeader(payload, + m_attributes.endpointGuid.prefix); + SequenceNumber_t last_valid = msg.gapStart; + --last_valid; + auto missing_sns = writer->getMissing(writer->expectedSN, last_valid); + rtps::MessageFactory::addAckNack(payload, msg.writerId, msg.readerId, + missing_sns, writer->getNextAckNackCount(), + false); + info.payload = std::move(payload.bytes); + if (info.payload.empty()) { + return false; + } + m_transport->sendPacket(info); + return true; + } + + // Case 2: We are expecting a message between [gapStart; gapList.base -1] + // Advance expectedSN beyond gapList.base + if (writer->expectedSN < msg.gapList.base) { + writer->expectedSN = msg.gapList.base; + + // writer->expectedSN++; + + // Advance expectedSN to first unset bit + for (uint32_t bit = 0; bit < SNS_MAX_NUM_BITS; + writer->expectedSN++, bit++) { + if (!msg.gapList.isSet(bit)) { + break; + } + } + + return true; + + }else{ + + // Case 3: We are expecting a sequence number beyond gap list base, + // check if we need to update expectedSN + auto i = msg.gapList.base; + for(uint32_t bit = 0; bit < SNS_MAX_NUM_BITS; i++, bit++){ + if(i < writer->expectedSN){ + continue; + } + + if(msg.gapList.isSet(bit)){ + writer->expectedSN++; + }else{ + PacketInfo info; + info.srcPort = m_srcPort; + info.destAddr = writer->remoteLocator.getIp4AddressBytes(); + info.destPort = writer->remoteLocator.port; + PayloadBuffer payload; + rtps::MessageFactory::addHeader(payload, + m_attributes.endpointGuid.prefix); + SequenceNumberSet set; + set.base = writer->expectedSN; + set.numBits = 1; + set.bitMap[0] = set.bitMap[0] |= uint32_t{1} << 31; + rtps::MessageFactory::addAckNack(payload, msg.writerId, msg.readerId, + set, writer->getNextAckNackCount(), + false); + info.payload = std::move(payload.bytes); + if (info.payload.empty()) { + return false; + } + m_transport->sendPacket(info); + + return true; + } + } + + + return false; + } +} + +template +bool StatefulReaderT::onNewHeartbeat( + const SubmessageHeartbeat &msg, const GuidPrefix_t &sourceGuidPrefix) { + std::lock_guard lock(m_proxies_mutex); + if (!m_is_initialized_) { + return false; + } + PacketInfo info; + info.srcPort = m_srcPort; + PayloadBuffer payload; + + Guid_t writerProxyGuid; + writerProxyGuid.prefix = sourceGuidPrefix; + writerProxyGuid.entityId = msg.writerId; + WriterProxy *writer = getProxy(writerProxyGuid); + + if (writer == nullptr) { + +#if SFR_VERBOSE && RTPS_GLOBAL_VERBOSE + SFR_LOG("Ignore heartbeat. Couldn't find a matching writer"); +#endif + return false; + } + + if (writer->expectedSN < msg.firstSN) { + SFR_LOG("expectedSN < firstSN, advancing expectedSN"); + writer->expectedSN = msg.firstSN; + } + + writer->hbCount.value = msg.count.value; + info.destAddr = writer->remoteLocator.getIp4AddressBytes(); + info.destPort = writer->remoteLocator.port; + rtps::MessageFactory::addHeader(payload, + m_attributes.endpointGuid.prefix); + auto missing_sns = writer->getMissing(msg.firstSN, msg.lastSN); + bool final_flag = (missing_sns.numBits == 0); + rtps::MessageFactory::addAckNack(payload, msg.writerId, msg.readerId, + missing_sns, writer->getNextAckNackCount(), + final_flag); + + SFR_LOG("Sending acknack base {} bits {}.", (int)missing_sns.base.low, + (int)missing_sns.numBits); + info.payload = std::move(payload.bytes); + if (info.payload.empty()) { + return false; + } + m_transport->sendPacket(info); + return true; +} + +template +bool StatefulReaderT::sendPreemptiveAckNack( + const WriterProxy &writer) { + std::lock_guard lock(m_proxies_mutex); + if (!m_is_initialized_) { + return false; + } + + PacketInfo info; + info.srcPort = m_attributes.unicastLocator.port; + info.destAddr = writer.remoteLocator.getIp4AddressBytes(); + info.destPort = writer.remoteLocator.port; + PayloadBuffer payload; + rtps::MessageFactory::addHeader(payload, + m_attributes.endpointGuid.prefix); + SequenceNumberSet number_set; + number_set.base.high = 0; + number_set.base.low = 0; + number_set.numBits = 0; + rtps::MessageFactory::addAckNack( + payload, writer.remoteWriterGuid.entityId, + m_attributes.endpointGuid.entityId, number_set, Count_t{1}, false); + + SFR_LOG("Sending preemptive acknack."); + info.payload = std::move(payload.bytes); + if (info.payload.empty()) { + return false; + } + m_transport->sendPacket(info); + return true; +} diff --git a/components/rtps_embedded/include/rtps/entities/StatefulWriter.h b/components/rtps_embedded/include/rtps/entities/StatefulWriter.h new file mode 100644 index 000000000..a5ff6b204 --- /dev/null +++ b/components/rtps_embedded/include/rtps/entities/StatefulWriter.h @@ -0,0 +1,97 @@ +/* +The MIT License +Copyright (c) 2019 Lehrstuhl Informatik 11 - RWTH Aachen University +Modifications Copyright (c) 2026 ATDev +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: +The above copyright notice and this permission notice shall be included in +all copies or substantial portions of the Software. +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +THE SOFTWARE + +This file is part of embeddedRTPS. + +Author: i11 - Embedded Software, RWTH Aachen University +*/ + +#ifndef RTPS_STATEFULWRITER_H +#define RTPS_STATEFULWRITER_H + +#include "rtps/entities/ReaderProxy.h" +#include "rtps/entities/Writer.h" +#include "rtps/common/types.h" +#include "rtps/storages/HistoryCacheWithDeletion.h" +#include "rtps/storages/MemoryPool.h" +#include "task.hpp" + +#include + +namespace rtps { + +class EsppTransport; + +template class StatefulWriterT final : public Writer { +public: + ~StatefulWriterT() override; + bool init(TopicData attributes, TopicKind_t topicKind, ThreadPool *threadPool, + NetworkDriver &driver, bool enfUnicast = false); + + //! Executes required steps like sending packets. Intended to be called by + //! worker threads + void progress() override; + const CacheChange *newChange(ChangeKind_t kind, const uint8_t *data, + DataSize_t size, bool inLineQoS = false, + bool markDisposedAfterWrite = false) override; + + bool removeFromHistory(const SequenceNumber_t &s); + void setAllChangesToUnsent() override; + void onNewAckNack(const SubmessageAckNack &msg, + const GuidPrefix_t &sourceGuidPrefix) override; + void reset() override; + void updateChangeKind(SequenceNumber_t &sequence_number); + +private: + NetworkDriver *m_transport; + + HistoryCacheWithDeletion m_history; + + /* + * Cache changes marked as disposeAfterWrite are retained for a short amount + * in case of retransmission The whole 'disposeAfterWrite' mechanisms only + * exists to allow for repeated creation and deletion of endpoints during + * operation. Otherwise the history will quickly reach its limits. Will be + * replaced with something more elegant in the future. + */ + ThreadSafeCircularBuffer m_disposeWithDelay; + void dropDisposeAfterWriteChanges(); + + std::unique_ptr m_heartbeatTask; + + Count_t m_hbCount{1}; + + bool m_running = true; + bool m_thread_running = false; + + bool sendData(const ReaderProxy &reader, const CacheChange *next); + bool sendDataWRMulticast(const ReaderProxy &reader, const CacheChange *next); + void sendHeartBeatLoop(); + void sendHeartBeat(); + void sendGap(const ReaderProxy &reader, const SequenceNumber_t &firstMissing, + const SequenceNumber_t &nextValid); +}; + +using StatefulWriter = StatefulWriterT; +} // namespace rtps + +#include "StatefulWriter.tpp" + +#endif // RTPS_STATEFULWRITER_H diff --git a/components/rtps_embedded/include/rtps/entities/StatefulWriter.tpp b/components/rtps_embedded/include/rtps/entities/StatefulWriter.tpp new file mode 100644 index 000000000..fe370107b --- /dev/null +++ b/components/rtps_embedded/include/rtps/entities/StatefulWriter.tpp @@ -0,0 +1,595 @@ +/* +The MIT License +Copyright (c) 2019 Lehrstuhl Informatik 11 - RWTH Aachen University +Modifications Copyright (c) 2026 ATDev +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: +The above copyright notice and this permission notice shall be included in +all copies or substantial portions of the Software. +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +THE SOFTWARE + +This file is part of embeddedRTPS. + +Author: i11 - Embedded Software, RWTH Aachen University +*/ + +#include "rtps/entities/StatefulWriter.h" +#include "rtps/messages/MessageFactory.h" +#include "rtps/messages/MessageTypes.h" +#include "rtps/storages/PayloadBuffer.h" +#include "rtps/utils/Log.h" +#include +#include +#include +#include +#include + +using rtps::StatefulWriterT; +using rtps::CacheChange; +using rtps::GuidPrefix_t; +using rtps::ReaderProxy; +using rtps::SequenceNumber_t; +using rtps::SubmessageAckNack; + +#if SFW_VERBOSE && RTPS_GLOBAL_VERBOSE +#include "rtps/utils/printutils.h" +#define SFW_LOG(...) logger_.warn(__VA_ARGS__) +#else +#define SFW_LOG(...) do { } while (0) +#endif + +template +StatefulWriterT::~StatefulWriterT() { + m_running = false; + if (m_heartbeatTask) { + m_heartbeatTask->stop(); + } + while (m_thread_running) { + // Wait for the heartbeat loop to observe m_running and exit. + std::this_thread::sleep_for(std::chrono::milliseconds(500)); + } +} + +template +bool StatefulWriterT::init(TopicData attributes, + TopicKind_t topicKind, + ThreadPool *threadPool, + NetworkDriver &driver, + bool enfUnicast) { + + m_attributes = attributes; + + mp_threadPool = threadPool; + m_srcPort = attributes.unicastLocator.port; + m_enforceUnicast = enfUnicast; + m_topicKind = topicKind; + + m_nextSequenceNumberToSend = {0, 1}; + m_proxies.clear(); + + m_transport = &driver; + m_history.clear(); + m_hbCount = {1}; + + if (!m_disposeWithDelay.init()) { + SFW_LOG("Failed to initialize delayed dispose buffer mutex."); + return false; + } + + // Thread already exists, do not create new one (reusing slot case) + m_is_initialized_ = true; + + if (!m_thread_running) { + + m_running = true; + m_thread_running = false; + + const char *task_name = "HBThread"; + if (m_attributes.endpointGuid.entityId == + ENTITYID_SEDP_BUILTIN_PUBLICATIONS_WRITER) { + task_name = "HBThreadPub"; + } else if (m_attributes.endpointGuid.entityId == + ENTITYID_SEDP_BUILTIN_SUBSCRIPTIONS_WRITER) { + task_name = "HBThreadSub"; + } + + if (!m_heartbeatTask) { + espp::Task::Config config; + config.callback = [this]() { + sendHeartBeatLoop(); + return true; + }; + config.task_config.name = task_name; + config.task_config.stack_size_bytes = Config::HEARTBEAT_STACKSIZE; + config.task_config.priority = Config::THREAD_POOL_WRITER_PRIO; + config.log_level = espp::Logger::Verbosity::WARN; + m_heartbeatTask = espp::Task::make_unique(config); + } + + (void)m_heartbeatTask->start(); + } + + return true; +} + +template void StatefulWriterT::reset() { + m_is_initialized_ = false; + // TODO +} + +template +const rtps::CacheChange *StatefulWriterT::newChange( + ChangeKind_t kind, const uint8_t *data, DataSize_t size, bool inLineQoS, + bool markDisposedAfterWrite) { + INIT_GUARD() + if (isIrrelevant(kind)) { + return nullptr; + } + + std::lock_guard lock(m_mutex); + if (!m_is_initialized_) { + return nullptr; + } + + if (m_history.isFull()) { + // Right now we drop elements anyway because we cannot detect non-responding + // readers yet. return nullptr; + SequenceNumber_t newMin = + ++SequenceNumber_t(m_history.getCurrentSeqNumMin()); + if (m_nextSequenceNumberToSend < newMin) { + m_nextSequenceNumberToSend = + newMin; // Make sure we have the correct sn to send + } + SFW_LOG("History full! Dropping changes {}.", this->m_attributes.topicName); + } + + auto *result = + m_history.addChange(data, size, inLineQoS, markDisposedAfterWrite); + if (mp_threadPool != nullptr) { + mp_threadPool->addWorkload(this); + } + + SFW_LOG("Adding new data."); + + return result; +} + +template void StatefulWriterT::progress() { + INIT_GUARD() + std::lock_guard lock(m_mutex); + CacheChange *next = m_history.getChangeBySN(m_nextSequenceNumberToSend); + if (next != nullptr) { + uint32_t i = 0; + for (const auto &proxy : m_proxies) { + if (!m_enforceUnicast) { + sendDataWRMulticast(proxy, next); + } else { + i++; + sendData(proxy, next); + } + } + + SFW_LOG("Sending data with SN {}.{}", (int)m_nextSequenceNumberToSend.low, + (int)m_nextSequenceNumberToSend.high); + + if (next->disposeAfterWrite) { + SFW_LOG("Dispose after write msg sent to {} proxies", (int)i); + } + + /* + * Use case: deletion of local endpoints + * -> send Data Message with Disposed Flag set + * -> Set respective SEDP CacheChange as NOT_ALIVE_DISPOSED after + * transmission to proxies + * -> onAckNack will send Gap Messages to skip deleted local endpoints + * during SEDP + */ + if (next->disposeAfterWrite) { + next->sentTime = std::chrono::steady_clock::now(); + if (!m_disposeWithDelay.copyElementIntoBuffer(next->sequenceNumber)) { + SFW_LOG("Failed to enqueue dispose after write!"); + m_history.dropChange(next->sequenceNumber); + } else { + SFW_LOG("Delayed dispose scheduled for sn {} {}", + (int)next->sequenceNumber.high, (int)next->sequenceNumber.low); + } + } + + ++m_nextSequenceNumberToSend; + SFW_LOG("HB from progress"); + sendHeartBeat(); + + } else { + SFW_LOG("Couldn't get a CacheChange with SN ({},{})", + m_nextSequenceNumberToSend.high, m_nextSequenceNumberToSend.low); + } +} + +template +void StatefulWriterT::setAllChangesToUnsent() { + INIT_GUARD() + std::lock_guard lock(m_mutex); + + m_nextSequenceNumberToSend = m_history.getCurrentSeqNumMin(); + + if (mp_threadPool != nullptr) { + mp_threadPool->addWorkload(this); + } +} + +template +void StatefulWriterT::onNewAckNack( + const SubmessageAckNack &msg, const GuidPrefix_t &sourceGuidPrefix) { + INIT_GUARD() + std::lock_guard lock(m_mutex); + if (!m_is_initialized_) { + return; + } + + ReaderProxy *reader = nullptr; + for (auto &proxy : m_proxies) { + if (proxy.remoteReaderGuid.prefix == sourceGuidPrefix && + proxy.remoteReaderGuid.entityId == msg.readerId) { + reader = &proxy; + break; + } + } + + if (reader == nullptr) { +#if SFW_VERBOSE && RTPS_GLOBAL_VERBOSE + SFW_LOG("No proxy found with id: "); + printEntityId(msg.readerId); + SFW_LOG(" Dropping acknack.\n"); +#endif + return; + } + + reader->ackNackCount = msg.count; + reader->finalFlag = msg.header.finalFlag(); + reader->lastAckNackSequenceNumber = msg.readerSNState.base; + + rtps::SequenceNumber_t nextSN = msg.readerSNState.base; + + // Preemptive ack nack + if (nextSN.low == 0 && nextSN.high == 0) { + SFW_LOG("Received preemptive acknack, sending heartbeat."); + sendHeartBeat(); + return; + } + + if (m_history.isEmpty()) { + // We have never sent anything. Do not immediately respond with another + // heartbeat here, otherwise reader/writer can get stuck in HB<->ACKNACK + // ping-pong. Periodic heartbeat still handles liveliness. + if (m_history.getLastUsedSequenceNumber() == rtps::SequenceNumber_t{0, 0}) { + SFW_LOG("Ignoring acknack while history is empty and no samples were sent yet."); + return; + } else { + // No data but we have sent something in the past -> GapStart = + // readerSNState.base, NextValid = lastUsedSequenceNumber+1 + rtps::SequenceNumber_t nextValid = m_history.getLastUsedSequenceNumber(); + ++nextValid; + sendGap(*reader, msg.readerSNState.base, nextValid); + } + + return; + } + + // Requesting smaller SN than minimum sequence number -> sendGap + if (msg.readerSNState.base < m_history.getCurrentSeqNumMin()) { + sendGap(*reader, msg.readerSNState.base, m_history.getCurrentSeqNumMin()); + return; + } + + SFW_LOG("Received non-preemptive acknack with {} bits set.", + msg.readerSNState.numBits); + for (uint32_t i = 0; i < msg.readerSNState.numBits && + nextSN <= m_history.getLastUsedSequenceNumber(); + ++i, ++nextSN) { + + if (msg.readerSNState.isSet(i)) { + + SFW_LOG("Looking for change {} | Bit {}", nextSN.low, i); + const rtps::CacheChange *cache = m_history.getChangeBySN(nextSN); + + // We still have the cache, send DATA + if (cache != nullptr) { + if (cache->disposeAfterWrite) { + SFW_LOG("Serving from dispose-after-write cache"); + } + sendData(*reader, cache); + } else { + SFW_LOG("> Change not found, search for next valid SN {}", + nextSN.low); + // Cache not found, look for next valid SN + rtps::SequenceNumber_t gapBegin = nextSN; + rtps::CacheChange *nextValidChange = nullptr; + uint32_t j = i + 1; + for (++nextSN; nextSN <= m_history.getLastUsedSequenceNumber(); + ++nextSN, ++j) { + nextValidChange = m_history.getChangeBySN(nextSN); + if (nextValidChange != nullptr) { + break; + } + } + if (nextValidChange == nullptr) { + sendGap(*reader, gapBegin, nextSN); + return; + } else { + sendGap(*reader, gapBegin, nextValidChange->sequenceNumber); + } + // sendData(nullptr, nextValidChange); + nextSN = nextValidChange->sequenceNumber; + --nextSN; + i = --j; + } + } + } +} + +template +bool rtps::StatefulWriterT::removeFromHistory( + const SequenceNumber_t &s) { + std::lock_guard lock(m_mutex); + return m_history.dropChange(s); +} + +template +bool StatefulWriterT::sendData(const ReaderProxy &reader, + const CacheChange *next) { + INIT_GUARD() + // TODO smarter packaging, e.g. create a message struct and serialize once. + + PacketInfo info; + info.srcPort = m_srcPort; + PayloadBuffer payload; + + MessageFactory::addHeader(payload, m_attributes.endpointGuid.prefix); + MessageFactory::addSubMessageTimeStamp(payload); + + // Just usable for IPv4 + const LocatorIPv4 &locator = reader.remoteLocator; + + info.destAddr = locator.getIp4AddressBytes(); + info.destPort = (Ip4Port_t)locator.port; + + MessageFactory::addSubMessageData( + payload, next->data, next->inLineQoS, next->sequenceNumber, + m_attributes.endpointGuid.entityId, reader.remoteReaderGuid.entityId); + info.payload = std::move(payload.bytes); + if (info.payload.empty()) { + return false; + } + m_transport->sendPacket(info); + + return true; +} + +template +void StatefulWriterT::sendGap( + const ReaderProxy &reader, const SequenceNumber_t &firstMissing, + const SequenceNumber_t &nextValid) { + INIT_GUARD() + // TODO smarter packaging, e.g. create a message struct and serialize once. + + PacketInfo info; + info.srcPort = m_srcPort; + PayloadBuffer payload; + + MessageFactory::addHeader(payload, m_attributes.endpointGuid.prefix); + MessageFactory::addSubMessageTimeStamp(payload); + + // Just usable for IPv4 + const LocatorIPv4 &locator = reader.remoteLocator; + + info.destAddr = locator.getIp4AddressBytes(); + info.destPort = (Ip4Port_t)locator.port; + + MessageFactory::addSubmessageGap( + payload, m_attributes.endpointGuid.entityId, + reader.remoteReaderGuid.entityId, firstMissing, nextValid); + info.payload = std::move(payload.bytes); + if (info.payload.empty()) { + return; + } + m_transport->sendPacket(info); +} + +template +bool StatefulWriterT::sendDataWRMulticast( + const ReaderProxy &reader, const CacheChange *next) { + INIT_GUARD() + + if (reader.useMulticast || reader.suppressUnicast == false) { + PacketInfo info; + info.srcPort = m_srcPort; + PayloadBuffer payload; + + MessageFactory::addHeader(payload, m_attributes.endpointGuid.prefix); + MessageFactory::addSubMessageTimeStamp(payload); + + // Decide whether to use multicast or unicast. + if (reader.useMulticast) { + const LocatorIPv4 &locator = reader.remoteMulticastLocator; + info.destAddr = locator.getIp4AddressBytes(); + info.destPort = (Ip4Port_t)locator.port; + } else { + const LocatorIPv4 &locator = reader.remoteLocator; + info.destAddr = locator.getIp4AddressBytes(); + info.destPort = (Ip4Port_t)locator.port; + } + + EntityId_t reid; + if (reader.useMulticast) { + reid = ENTITYID_UNKNOWN; + } else { + reid = reader.remoteReaderGuid.entityId; + } + + MessageFactory::addSubMessageData(payload, next->data, next->inLineQoS, + next->sequenceNumber, + m_attributes.endpointGuid.entityId, reid); + + info.payload = std::move(payload.bytes); + if (info.payload.empty()) { + return false; + } + m_transport->sendPacket(info); + } + return true; +} + +template +void StatefulWriterT::sendHeartBeatLoop() { + m_thread_running = true; + while (m_running) { + SFW_LOG("HB from loop"); + sendHeartBeat(); + dropDisposeAfterWriteChanges(); + bool unconfirmed_changes = false; + for (auto it : m_proxies) { + if (it.lastAckNackSequenceNumber < m_nextSequenceNumberToSend) { + unconfirmed_changes = true; + break; + } + } + + // Temporarily increase HB frequency if there are unconfirmed remote changes + if (unconfirmed_changes) { + SFW_LOG("HB speedup"); + std::this_thread::sleep_for( + std::chrono::milliseconds(Config::SF_WRITER_HB_PERIOD_MS / 4)); + } else { + std::this_thread::sleep_for( + std::chrono::milliseconds(Config::SF_WRITER_HB_PERIOD_MS)); + } + } + m_thread_running = false; +} + +template +void StatefulWriterT::dropDisposeAfterWriteChanges() { + SequenceNumber_t oldest_retained; + while (m_disposeWithDelay.peakFirst(oldest_retained)) { + + CacheChange *change = m_history.getChangeBySN(oldest_retained); + if (change == nullptr || !change->disposeAfterWrite) { + // Not in history anymore, drop + m_disposeWithDelay.moveFirstInto(oldest_retained); + return; + } + + if (change->sentTime == CacheChange::TimePoint{}) { + m_history.dropChange(change->sequenceNumber); + SequenceNumber_t tmp; + m_disposeWithDelay.moveFirstInto(tmp); + continue; + } + + auto age = std::chrono::steady_clock::now() - change->sentTime; + if (age > std::chrono::milliseconds(4000)) { + m_history.dropChange(change->sequenceNumber); + SFW_LOG("Removing SN {} {} for good", + static_cast(oldest_retained.low), + static_cast(oldest_retained.high)); + SequenceNumber_t tmp; + m_disposeWithDelay.moveFirstInto(tmp); + + continue; + } else { + return; + } + } +} + +template +void StatefulWriterT::sendHeartBeat() { + INIT_GUARD() + if (m_proxies.isEmpty() || !m_is_initialized_) { + + SFW_LOG("Skipping heartbeat. No proxies."); + return; + } + + for (auto &proxy : m_proxies) { + + PacketInfo info; + info.srcPort = m_srcPort; + PayloadBuffer payload; + + SequenceNumber_t firstSN; + SequenceNumber_t lastSN; + + MessageFactory::addHeader(payload, m_attributes.endpointGuid.prefix); + + { + std::lock_guard lock(m_mutex); + + if (!m_history.isEmpty()) { + firstSN = m_history.getCurrentSeqNumMin(); + lastSN = m_history.getCurrentSeqNumMax(); + + // Otherwise we may announce changes that have not been sent at least + // once! + if (lastSN > m_nextSequenceNumberToSend || + lastSN == m_nextSequenceNumberToSend) { + lastSN = m_nextSequenceNumberToSend; + --lastSN; + } + + // Proxy has confirmed all sequence numbers and set final flag + if ((proxy.lastAckNackSequenceNumber > lastSN) && proxy.finalFlag && + proxy.ackNackCount.value > 0) { + SFW_LOG("Skipping heartbeat for proxy, all changes confirmed. lastSN {}.{}, lastAckNack {}.{}", + (int)lastSN.low, + (int)lastSN.high, + (int)proxy.lastAckNackSequenceNumber.low, + (int)proxy.lastAckNackSequenceNumber.high); + continue; + } + } else if (m_history.getLastUsedSequenceNumber() == + SequenceNumber_t{0, 0}) { + if ((proxy.lastAckNackSequenceNumber > m_history.getLastUsedSequenceNumber()) && proxy.finalFlag && + proxy.ackNackCount.value > 0) { + SFW_LOG("Skipping heartbeat for proxy, all changes confirmed. lastUsedSN {}.{}, lastAckNack {}.{}", + (int)m_history.getLastUsedSequenceNumber().low, + (int)m_history.getLastUsedSequenceNumber().high, + (int)proxy.lastAckNackSequenceNumber.low, + (int)proxy.lastAckNackSequenceNumber.high); + continue; + } + firstSN = SequenceNumber_t{0, 1}; + lastSN = SequenceNumber_t{0, 0}; + } else { + firstSN = SequenceNumber_t{0, 1}; + lastSN = m_history.getLastUsedSequenceNumber(); + } + } + + SFW_LOG("Sending HB with SN range [{}.{};{}.{}]", firstSN.low, firstSN.high, + lastSN.low, lastSN.high); + + MessageFactory::addHeartbeat( + payload, m_attributes.endpointGuid.entityId, + proxy.remoteReaderGuid.entityId, firstSN, lastSN, m_hbCount); + + info.destAddr = proxy.remoteLocator.getIp4AddressBytes(); + info.destPort = proxy.remoteLocator.port; + info.payload = std::move(payload.bytes); + if (info.payload.empty()) { + continue; + } + m_transport->sendPacket(info); + } + m_hbCount.value++; +} diff --git a/components/rtps_embedded/include/rtps/entities/StatelessReader.h b/components/rtps_embedded/include/rtps/entities/StatelessReader.h new file mode 100644 index 000000000..6ab9759cf --- /dev/null +++ b/components/rtps_embedded/include/rtps/entities/StatelessReader.h @@ -0,0 +1,45 @@ +/* +The MIT License +Copyright (c) 2019 Lehrstuhl Informatik 11 - RWTH Aachen University +Modifications Copyright (c) 2026 ATDev +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: +The above copyright notice and this permission notice shall be included in +all copies or substantial portions of the Software. +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +THE SOFTWARE + +This file is part of embeddedRTPS. + +Author: i11 - Embedded Software, RWTH Aachen University +*/ + +#ifndef RTPS_STATELESSREADER_H +#define RTPS_STATELESSREADER_H + +#include "rtps/entities/Reader.h" + +namespace rtps { +class StatelessReader final : public Reader { +public: + bool init(const TopicData &attributes); + void newChange(const ReaderCacheChange &cacheChange) override; + bool onNewHeartbeat(const SubmessageHeartbeat &msg, + const GuidPrefix_t &remotePrefix) override; + bool addNewMatchedWriter(const WriterProxy &newProxy) override; + bool onNewGapMessage(const SubmessageGap &msg, + const GuidPrefix_t &remotePrefix) override; +}; + +} // namespace rtps + +#endif // RTPS_STATELESSREADER_H diff --git a/components/rtps_embedded/include/rtps/entities/StatelessWriter.h b/components/rtps_embedded/include/rtps/entities/StatelessWriter.h new file mode 100644 index 000000000..2cf07e692 --- /dev/null +++ b/components/rtps_embedded/include/rtps/entities/StatelessWriter.h @@ -0,0 +1,69 @@ +/* +The MIT License +Copyright (c) 2019 Lehrstuhl Informatik 11 - RWTH Aachen University +Modifications Copyright (c) 2026 ATDev +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: +The above copyright notice and this permission notice shall be included in +all copies or substantial portions of the Software. +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +THE SOFTWARE + +This file is part of embeddedRTPS. + +Author: i11 - Embedded Software, RWTH Aachen University +*/ + +#ifndef RTPS_RTPSWRITER_H +#define RTPS_RTPSWRITER_H + +#include "rtps/common/types.h" +#include "rtps/config.h" +#include "rtps/entities/Writer.h" +#include "rtps/common/types.h" +#include "rtps/storages/MemoryPool.h" +#include "rtps/storages/SimpleHistoryCache.h" + +namespace rtps { + +class EsppTransport; + +template class StatelessWriterT : public Writer { +public: + ~StatelessWriterT() override; + bool init(TopicData attributes, TopicKind_t topicKind, ThreadPool *threadPool, + NetworkDriver &driver, bool enfUnicast = false); + + void progress() override; + const CacheChange *newChange(ChangeKind_t kind, const uint8_t *data, + DataSize_t size, bool inLineQoS = false, + bool markDisposedAfterWrite = false) override; + bool removeFromHistory(const SequenceNumber_t &s); + + void setAllChangesToUnsent() override; + void onNewAckNack(const SubmessageAckNack &msg, + const GuidPrefix_t &sourceGuidPrefix) override; + void reset() override; + +private: + NetworkDriver *m_transport; + + SimpleHistoryCache m_history; +}; + +using StatelessWriter = StatelessWriterT; + +} // namespace rtps + +#include "StatelessWriter.tpp" + +#endif // RTPS_RTPSWRITER_H diff --git a/components/rtps_embedded/include/rtps/entities/StatelessWriter.tpp b/components/rtps_embedded/include/rtps/entities/StatelessWriter.tpp new file mode 100644 index 000000000..0f883d5ad --- /dev/null +++ b/components/rtps_embedded/include/rtps/entities/StatelessWriter.tpp @@ -0,0 +1,219 @@ +/* +The MIT License +Copyright (c) 2019 Lehrstuhl Informatik 11 - RWTH Aachen University +Modifications Copyright (c) 2026 ATDev +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: +The above copyright notice and this permission notice shall be included in +all copies or substantial portions of the Software. +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +THE SOFTWARE + +This file is part of embeddedRTPS. + +Author: i11 - Embedded Software, RWTH Aachen University +*/ + +#include +#include + +#include "rtps/ThreadPool.h" +#include "rtps/messages/MessageFactory.h" +#include "rtps/storages/PayloadBuffer.h" +#include "rtps/utils/Log.h" +#include "rtps/utils/udpUtils.h" +#include + +using rtps::CacheChange; +using rtps::GuidPrefix_t; +using rtps::SequenceNumber_t; +using rtps::StatelessWriterT; +using rtps::SubmessageAckNack; + +#if SLW_VERBOSE && RTPS_GLOBAL_VERBOSE +#include "rtps/utils/printutils.h" +#define SLW_LOG(...) logger_.warn(__VA_ARGS__) +#else +#define SLW_LOG(...) do { } while (0) +#endif + +template +StatelessWriterT::~StatelessWriterT() { + // if(sys_mutex_valid(&m_mutex)){ + // sys_mutex_free(&m_mutex); + // } +} + +template +bool StatelessWriterT::init(TopicData attributes, + TopicKind_t topicKind, + ThreadPool *threadPool, + NetworkDriver &driver, + bool enfUnicast) { + + m_attributes = attributes; + + mp_threadPool = threadPool; + m_srcPort = attributes.unicastLocator.port; + m_enforceUnicast = enfUnicast; + + m_topicKind = topicKind; + m_nextSequenceNumberToSend = {0, 1}; + m_is_initialized_ = true; + + m_proxies.clear(); + m_history.clear(); + + m_transport = &driver; + + return true; +} + +template +void StatelessWriterT::reset() { + m_is_initialized_ = false; +} + +template +const CacheChange *StatelessWriterT::newChange( + rtps::ChangeKind_t kind, const uint8_t *data, DataSize_t size, + bool inLineQoS, bool markDisposedAfterWrite) { + INIT_GUARD(); + if (isIrrelevant(kind)) { + return nullptr; + } + std::lock_guard lock(m_mutex); + if (!m_is_initialized_) { + return nullptr; + } + + if (m_history.isFull()) { + SequenceNumber_t newMin = ++SequenceNumber_t(m_history.getSeqNumMin()); + if (m_nextSequenceNumberToSend < newMin) { + m_nextSequenceNumberToSend = + newMin; // Make sure we have the correct sn to send + } + SLW_LOG("History is full, dropping oldest {}", this->m_attributes.topicName); + } + + auto *result = m_history.addChange(data, size); + if (mp_threadPool != nullptr) { + mp_threadPool->addWorkload(this); + } + + SLW_LOG("Adding new data."); + return result; +} + +template +bool StatelessWriterT::removeFromHistory( + const SequenceNumber_t &s) { + return false; // Stateless Writers currently do not support deletion from + // history +} + +template +void StatelessWriterT::setAllChangesToUnsent() { + INIT_GUARD(); + std::lock_guard lock(m_mutex); + + m_nextSequenceNumberToSend = m_history.getSeqNumMin(); + + if (mp_threadPool != nullptr) { + mp_threadPool->addWorkload(this); + } +} + +template +void StatelessWriterT::onNewAckNack( + const SubmessageAckNack & /*msg*/, const GuidPrefix_t &sourceGuidPrefix) { + INIT_GUARD(); + // Too lazy to respond +} + +template +void StatelessWriterT::progress() { + INIT_GUARD(); + // TODO smarter packaging e.g. by creating MessageStruct and serializing + // after adjusting values. + + if (m_proxies.getNumElements() == 0) { + SLW_LOG("No proxy!"); + } + + for (const auto &proxy : m_proxies) { + + SLW_LOG("Progress."); + // Do nothing, if someone else sends for me... (Multicast) + if (proxy.useMulticast || !proxy.suppressUnicast || m_enforceUnicast) { + PacketInfo info; + info.srcPort = m_srcPort; + PayloadBuffer payload; + + MessageFactory::addHeader(payload, m_attributes.endpointGuid.prefix); + MessageFactory::addSubMessageTimeStamp(payload); + + { + std::lock_guard lock(m_mutex); + const CacheChange *next = + m_history.getChangeBySN(m_nextSequenceNumberToSend); + if (next == nullptr) { + SLW_LOG("Couldn't get a new CacheChange with SN " + "(%li,%li)\n", + m_nextSequenceNumberToSend.high, + m_nextSequenceNumberToSend.low); + return; + } else { + SLW_LOG("Sending change with SN ({},{})", + m_nextSequenceNumberToSend.high, + m_nextSequenceNumberToSend.low); + } + + // Set EntityId to UNKNOWN if using multicast, because there might be + // different ones... + // TODO: mybe enhance by using UNKNOWN only if ids are really different + EntityId_t reid; + if (proxy.useMulticast && !m_enforceUnicast && proxy.unknown_eid) { + reid = ENTITYID_UNKNOWN; + } else { + reid = proxy.remoteReaderGuid.entityId; + } + MessageFactory::addSubMessageData(payload, next->data, false, + next->sequenceNumber, + m_attributes.endpointGuid.entityId, + reid); // TODO + } + + info.payload = std::move(payload.bytes); + + // Just usable for IPv4 + // Decide which locator to be used unicast/multicast + + if (proxy.useMulticast && !m_enforceUnicast) { + info.destAddr = proxy.remoteMulticastLocator.getIp4AddressBytes(); + info.destPort = (Ip4Port_t)proxy.remoteMulticastLocator.port; + } else { + info.destAddr = proxy.remoteLocator.getIp4AddressBytes(); + info.destPort = (Ip4Port_t)proxy.remoteLocator.port; + } + SLW_LOG("Sending to {}.{}.{}.{}:{}", info.destAddr[0], info.destAddr[1], + info.destAddr[2], info.destAddr[3], info.destPort); + if (info.payload.empty()) { + continue; + } + m_transport->sendPacket(info); + } + } + + m_history.removeUntilIncl(m_nextSequenceNumberToSend); + ++m_nextSequenceNumberToSend; +} diff --git a/components/rtps_embedded/include/rtps/entities/Writer.h b/components/rtps_embedded/include/rtps/entities/Writer.h new file mode 100644 index 000000000..2df11f834 --- /dev/null +++ b/components/rtps_embedded/include/rtps/entities/Writer.h @@ -0,0 +1,119 @@ +/* +The MIT License +Copyright (c) 2019 Lehrstuhl Informatik 11 - RWTH Aachen University +Modifications Copyright (c) 2026 ATDev +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: +The above copyright notice and this permission notice shall be included in +all copies or substantial portions of the Software. +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +THE SOFTWARE + +This file is part of embeddedRTPS. + +Author: i11 - Embedded Software, RWTH Aachen University +*/ + +#ifndef RTPS_WRITER_H +#define RTPS_WRITER_H + +#include "base_component.hpp" +#include "rtps/ThreadPool.h" +#include "rtps/discovery/TopicData.h" +#include "rtps/entities/ReaderProxy.h" +#include "rtps/storages/CacheChange.h" +#include "rtps/storages/MemoryPool.h" + +#include +#include + +#ifdef DEBUG_BUILD +#define COMPILE_INIT_GUARD +#endif + +#ifdef COMPILE_INIT_GUARD +#define INIT_GUARD() \ + if (!m_is_initialized_) { \ + logger_.error("Using uninitialized endpoint"); \ + while (1) { \ + } \ + } \ + } +#else +#define INIT_GUARD() // +#endif + +namespace rtps { + +class Writer : public espp::BaseComponent { +public: + TopicData m_attributes; + virtual bool addNewMatchedReader(const ReaderProxy &newProxy); + virtual bool removeProxy(const Guid_t &guid); + virtual void removeAllProxiesOfParticipant(const GuidPrefix_t &guidPrefix); + virtual void reset() = 0; + virtual const CacheChange *newChange(ChangeKind_t kind, const uint8_t *data, + DataSize_t size); + + //! Executes required steps like sending packets. Intended to be called by + //! worker threads + virtual void progress() = 0; + + virtual bool removeFromHistory(const SequenceNumber_t &s) = 0; + virtual void setAllChangesToUnsent() = 0; + virtual void onNewAckNack(const SubmessageAckNack &msg, + const GuidPrefix_t &sourceGuidPrefix) = 0; + + using dumpProxyCallback = void (*)(const Writer *writer, const ReaderProxy &, + void *arg); + + int dumpAllProxies(dumpProxyCallback target, void *arg); + + bool isInitialized(); + std::uint32_t getProxiesCount(); + + void setSEDPSequenceNumber(const SequenceNumber_t &sn); + const SequenceNumber_t &getSEDPSequenceNumber(); + + bool isBuiltinEndpoint(); + +protected: + Writer(); + SequenceNumber_t m_sedp_sequence_number; + + std::recursive_mutex m_mutex; + ThreadPool *mp_threadPool = nullptr; + + Ip4Port_t m_srcPort; + + bool m_enforceUnicast; + + TopicKind_t m_topicKind = TopicKind_t::NO_KEY; + SequenceNumber_t m_nextSequenceNumberToSend; + + friend class SEDPAgent; + virtual const CacheChange *newChange(ChangeKind_t kind, const uint8_t *data, + DataSize_t size, bool inLineQoS, + bool markDisposedAfterWrite) = 0; + + friend class SizeInspector; + bool m_is_initialized_ = false; + virtual ~Writer() = default; + MemoryPool m_proxies; + + void resetSendOptions(); + void manageSendOptions(); + bool isIrrelevant(ChangeKind_t kind) const; +}; +} // namespace rtps + +#endif // RTPS_WRITER_H diff --git a/components/rtps_embedded/include/rtps/entities/WriterProxy.h b/components/rtps_embedded/include/rtps/entities/WriterProxy.h new file mode 100644 index 000000000..9d7bbf249 --- /dev/null +++ b/components/rtps_embedded/include/rtps/entities/WriterProxy.h @@ -0,0 +1,78 @@ +/* +The MIT License +Copyright (c) 2019 Lehrstuhl Informatik 11 - RWTH Aachen University +Modifications Copyright (c) 2026 ATDev +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: +The above copyright notice and this permission notice shall be included in +all copies or substantial portions of the Software. +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +THE SOFTWARE + +This file is part of embeddedRTPS. + +Author: i11 - Embedded Software, RWTH Aachen University +*/ + +#ifndef RTPS_WRITERPROXY_H +#define RTPS_WRITERPROXY_H + +#include "rtps/common/types.h" +#include + +namespace rtps { +struct WriterProxy { + Guid_t remoteWriterGuid; + SequenceNumber_t expectedSN; + Count_t ackNackCount; + Count_t hbCount; + bool is_reliable; + LocatorIPv4 remoteLocator; + + WriterProxy() = default; + + WriterProxy(const Guid_t &guid, const LocatorIPv4 &loc, bool reliable) + : remoteWriterGuid(guid), + expectedSN(SequenceNumber_t{0, 1}), ackNackCount{1}, hbCount{0}, + is_reliable(reliable), remoteLocator(loc) {} + + // For now, we don't store any packets, so we just request all starting from + // the next expected + SequenceNumberSet getMissing(const SequenceNumber_t &firstAvail, + const SequenceNumber_t &lastAvail) { + SequenceNumberSet set; + if (lastAvail < expectedSN) { + set.base = expectedSN; + set.numBits = 0; + } else { + set.base = expectedSN; + SequenceNumber_t i; + uint32_t bit; + for (bit = 0, i = expectedSN; i <= lastAvail && bit < SNS_MAX_NUM_BITS; + i++, bit++) { + set.bitMap[0] |= uint32_t{1} << (31 - bit); + set.numBits++; + } + } + + return set; + } + + Count_t getNextAckNackCount() { + const Count_t tmp = ackNackCount; + ++ackNackCount.value; + return tmp; + } +}; +} // namespace rtps + +#endif // RTPS_WRITERPROXY_H diff --git a/components/rtps_embedded/include/rtps/messages/MessageFactory.h b/components/rtps_embedded/include/rtps/messages/MessageFactory.h new file mode 100644 index 000000000..4bb45e3b3 --- /dev/null +++ b/components/rtps_embedded/include/rtps/messages/MessageFactory.h @@ -0,0 +1,220 @@ +/* +The MIT License +Copyright (c) 2019 Lehrstuhl Informatik 11 - RWTH Aachen University +Modifications Copyright (c) 2026 ATDev +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: +The above copyright notice and this permission notice shall be included in +all copies or substantial portions of the Software. +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +THE SOFTWARE + +This file is part of embeddedRTPS. + +Author: i11 - Embedded Software, RWTH Aachen University +*/ + +// Copyright 2023 Apex.AI, Inc. +// All rights reserved. + +#ifndef RTPS_MESSAGEFACTORY_H +#define RTPS_MESSAGEFACTORY_H + +#include "rtps/common/types.h" +#include "rtps/config.h" +#include "rtps/messages/MessageTypes.h" +#include "rtps/utils/sysFunctions.h" + +#include +#include + +namespace rtps { +namespace MessageFactory { +const std::array PROTOCOL_TYPE{'R', 'T', 'P', 'S'}; +const uint8_t numBytesUntilEndOfLength = + 4; // The first bytes incl. submessagelength don't count + +template +void addHeader(Buffer &buffer, const GuidPrefix_t &guidPrefix) { + + Header header; + header.protocolName = PROTOCOL_TYPE; + header.protocolVersion = PROTOCOLVERSION; + header.vendorId = Config::VENDOR_ID; + header.guidPrefix = guidPrefix; + + serializeMessage(buffer, header); +} + +template +bool addSubMessageInfoDST(Buffer &buffer, GuidPrefix_t &dst) { + SubmessageInfoDST msg; + msg.header.submessageId = SubmessageKind::INFO_DST; + +#if IS_LITTLE_ENDIAN + msg.header.flags = FLAG_LITTLE_ENDIAN; +#else + msg.header.flags = FLAG_BIG_ENDIAN; +#endif + + msg.header.octetsToNextHeader = sizeof(GuidPrefix_t); + msg.guidPrefix = dst; + + return serializeMessage(buffer, msg); +} + +template +void addSubMessageTimeStamp(Buffer &buffer, bool setInvalid = false) { + SubmessageHeader header; + header.submessageId = SubmessageKind::INFO_TS; + +#if IS_LITTLE_ENDIAN + header.flags = FLAG_LITTLE_ENDIAN; +#else + header.flags = FLAG_BIG_ENDIAN; +#endif + + if (setInvalid) { + header.flags |= FLAG_INVALIDATE; + header.octetsToNextHeader = 0; + } else { + header.octetsToNextHeader = sizeof(Time_t); + } + + serializeMessage(buffer, header); + + if (!setInvalid) { + buffer.reserve(header.octetsToNextHeader); + Time_t now = getCurrentTimeStamp(); + buffer.append(reinterpret_cast(&now.seconds), + sizeof(Time_t::seconds)); + buffer.append(reinterpret_cast(&now.fraction), + sizeof(Time_t::fraction)); + } +} + +template +void addSubMessageData(Buffer &buffer, const PayloadBuffer &filledPayload, + bool containsInlineQos, const SequenceNumber_t &SN, + const EntityId_t &writerID, const EntityId_t &readerID) { + SubmessageData msg; + msg.header.submessageId = SubmessageKind::DATA; +#if IS_LITTLE_ENDIAN + msg.header.flags = FLAG_LITTLE_ENDIAN; +#else + msg.header.flags = FLAG_BIG_ENDIAN; +#endif + + msg.header.octetsToNextHeader = SubmessageData::getRawSize() + + filledPayload.spaceUsed() - + numBytesUntilEndOfLength; + + if (containsInlineQos) { + msg.header.flags |= FLAG_INLINE_QOS; + } + if (filledPayload.isValid()) { + msg.header.flags |= FLAG_DATA_PAYLOAD; + } + + msg.writerSN = SN; + msg.extraFlags = 0; + msg.readerId = readerID; + msg.writerId = writerID; + + constexpr uint16_t octetsToInlineQoS = + 4 + 4 + 8; // EntityIds + SequenceNumber + msg.octetsToInlineQos = octetsToInlineQoS; + + serializeMessage(buffer, msg); + + if (filledPayload.isValid()) { + buffer.append(filledPayload); + } +} + +template +void addHeartbeat(Buffer &buffer, EntityId_t writerId, EntityId_t readerId, + SequenceNumber_t firstSN, SequenceNumber_t lastSN, + Count_t count) { + SubmessageHeartbeat subMsg; + subMsg.header.submessageId = SubmessageKind::HEARTBEAT; + subMsg.header.octetsToNextHeader = + SubmessageHeartbeat::getRawSize() - numBytesUntilEndOfLength; +#if IS_LITTLE_ENDIAN + subMsg.header.flags = FLAG_LITTLE_ENDIAN; +#else + subMsg.header.flags = FLAG_BIG_ENDIAN; +#endif + // Force response by not setting final flag. + + subMsg.writerId = writerId; + subMsg.readerId = readerId; + subMsg.firstSN = firstSN; + subMsg.lastSN = lastSN; + subMsg.count = count; + + serializeMessage(buffer, subMsg); +} + +template +void addAckNack(Buffer &buffer, EntityId_t writerId, EntityId_t readerId, + SequenceNumberSet readerSNState, Count_t count, + bool final_flag) { + SubmessageAckNack subMsg; + subMsg.header.submessageId = SubmessageKind::ACKNACK; +#if IS_LITTLE_ENDIAN + subMsg.header.flags = FLAG_LITTLE_ENDIAN; +#else + subMsg.header.flags = FLAG_BIG_ENDIAN; +#endif + if (final_flag) { + subMsg.header.flags |= FLAG_FINAL; // For now, we don't want any response + } else { + subMsg.header.flags &= + ~FLAG_FINAL; // Send future heartbeats, even if no change occured + } + subMsg.header.octetsToNextHeader = + SubmessageAckNack::getRawSize(readerSNState) - numBytesUntilEndOfLength; + + subMsg.writerId = writerId; + subMsg.readerId = readerId; + subMsg.readerSNState = readerSNState; + subMsg.count = count; + + serializeMessage(buffer, subMsg); +} + +template +void addSubmessageGap(Buffer &buffer, EntityId_t writerId, EntityId_t readerId, + const SequenceNumber_t &firstMissing, + const SequenceNumber_t &nextValid) { + SubmessageGap subMsg; + subMsg.header.submessageId = SubmessageKind::GAP; +#if IS_LITTLE_ENDIAN + subMsg.header.flags = FLAG_LITTLE_ENDIAN; +#else + subMsg.header.flags = FLAG_BIG_ENDIAN; +#endif + subMsg.header.octetsToNextHeader = 32; + + subMsg.writerId = writerId; + subMsg.readerId = readerId; + subMsg.gapStart = firstMissing; + subMsg.gapList.base = nextValid; + subMsg.gapList.numBits = 0; + + serializeMessage(buffer, subMsg); +} +} // namespace MessageFactory +} // namespace rtps + +#endif // RTPS_MESSAGEFACTORY_H diff --git a/components/rtps_embedded/include/rtps/messages/MessageReceiver.h b/components/rtps_embedded/include/rtps/messages/MessageReceiver.h new file mode 100644 index 000000000..ca39ca54d --- /dev/null +++ b/components/rtps_embedded/include/rtps/messages/MessageReceiver.h @@ -0,0 +1,76 @@ +/* +The MIT License +Copyright (c) 2019 Lehrstuhl Informatik 11 - RWTH Aachen University +Modifications Copyright (c) 2026 ATDev +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: +The above copyright notice and this permission notice shall be included in +all copies or substantial portions of the Software. +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +THE SOFTWARE + +This file is part of embeddedRTPS. + +Author: i11 - Embedded Software, RWTH Aachen University +*/ + +#ifndef RTPS_MESSAGERECEIVER_H +#define RTPS_MESSAGERECEIVER_H + +#include "base_component.hpp" +#include "rtps/common/types.h" +#include "rtps/config.h" +#include "rtps/discovery/BuiltInEndpoints.h" +#include + +namespace rtps { +class Reader; +class Writer; +class Participant; +class MessageProcessingInfo; + +class MessageReceiver : public espp::BaseComponent { +public: + GuidPrefix_t sourceGuidPrefix = GUIDPREFIX_UNKNOWN; + ProtocolVersion_t sourceVersion = PROTOCOLVERSION; + VendorId_t sourceVendor = VENDOR_UNKNOWN; + bool haveTimeStamp = false; + + explicit MessageReceiver(Participant *part); + + bool processMessage(const uint8_t *data, DataSize_t size); + +private: + Participant *mp_part; + + void resetState(); + + // TODO make msgInfo a member + // This probably make processing faster, as no parameter needs to be passed + // around However, we need to make sure data is set to nullptr after + // processMsg to make sure we don't access it again afterwards. + /** + * Check header for validity, modifies the state of the receiver and + * adjusts the position of msgInfo accordingly + */ + bool processGapSubmessage(MessageProcessingInfo &msgInfo); + bool processHeader(MessageProcessingInfo &msgInfo); + bool processSubmessage(MessageProcessingInfo &msgInfo, + const SubmessageHeader &submsgHeader); + bool processDataSubmessage(MessageProcessingInfo &msgInfo, + const SubmessageHeader &submsgHeader); + bool processHeartbeatSubmessage(MessageProcessingInfo &msgInfo); + bool processAckNackSubmessage(MessageProcessingInfo &msgInfo); +}; +} // namespace rtps + +#endif // RTPS_MESSAGERECEIVER_H diff --git a/components/rtps_embedded/include/rtps/messages/MessageTypes.h b/components/rtps_embedded/include/rtps/messages/MessageTypes.h new file mode 100644 index 000000000..208f5d144 --- /dev/null +++ b/components/rtps_embedded/include/rtps/messages/MessageTypes.h @@ -0,0 +1,455 @@ +/* +The MIT License +Copyright (c) 2019 Lehrstuhl Informatik 11 - RWTH Aachen University +Modifications Copyright (c) 2026 ATDev +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: +The above copyright notice and this permission notice shall be included in +all copies or substantial portions of the Software. +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +THE SOFTWARE + +This file is part of embeddedRTPS. + +Author: i11 - Embedded Software, RWTH Aachen University +*/ + +#ifndef RTPS_MESSAGES_H +#define RTPS_MESSAGES_H + +#include "rtps/common/types.h" + +#include +#include + +namespace rtps { + +#if defined(_MSC_VER) +#define RTPS_EMBEDDED_PACKED +#pragma pack(push, 1) +#else +#define RTPS_EMBEDDED_PACKED __attribute__((packed)) +#endif + +namespace SMElement { +// TODO endianess +enum ParameterId : uint16_t { + PID_PAD = 0x0000, + PID_SENTINEL = 0x0001, + PID_USER_DATA = 0x002c, + PID_TOPIC_NAME = 0x0005, + PID_TYPE_NAME = 0x0007, + PID_GROUP_DATA = 0x002d, + PID_TOPIC_DATA = 0x002e, + PID_DURABILITY = 0x001d, + PID_DURABILITY_SERVICE = 0x001e, + PID_DEADLINE = 0x0023, + PID_LATENCY_BUDGET = 0x0027, + PID_LIVELINESS = 0x001b, + PID_RELIABILITY = 0x001a, + PID_LIFESPAN = 0x002b, + PID_DESTINATION_ORDER = 0x0025, + PID_HISTORY = 0x0040, + PID_RESOURCE_LIMITS = 0x0041, + PID_OWNERSHIP = 0x001f, + PID_OWNERSHIP_STRENGTH = 0x0006, + PID_PRESENTATION = 0x0021, + PID_PARTITION = 0x0029, + PID_TIME_BASED_FILTER = 0x0004, + PID_TRANSPORT_PRIORITY = 0x0049, + PID_PROTOCOL_VERSION = 0x0015, + PID_VENDORID = 0x0016, + PID_UNICAST_LOCATOR = 0x002f, + PID_MULTICAST_LOCATOR = 0x0030, + PID_MULTICAST_IPADDRESS = 0x0011, + PID_DEFAULT_UNICAST_LOCATOR = 0x0031, + PID_DEFAULT_MULTICAST_LOCATOR = 0x0048, + PID_METATRAFFIC_UNICAST_LOCATOR = 0x0032, + PID_METATRAFFIC_MULTICAST_LOCATOR = 0x0033, + PID_DEFAULT_UNICAST_IPADDRESS = 0x000c, + PID_DEFAULT_UNICAST_PORT = 0x000e, + PID_METATRAFFIC_UNICAST_IPADDRESS = 0x0045, + PID_METATRAFFIC_UNICAST_PORT = 0x000d, + PID_METATRAFFIC_MULTICAST_IPADDRESS = 0x000b, + PID_METATRAFFIC_MULTICAST_PORT = 0x0046, + PID_EXPECTS_INLINE_QOS = 0x0043, + PID_PARTICIPANT_MANUAL_LIVELINESS_COUNT = 0x0034, + PID_PARTICIPANT_BUILTIN_ENDPOINTS = 0x0044, + PID_PARTICIPANT_LEASE_DURATION = 0x0002, + PID_CONTENT_FILTER_PROPERTY = 0x0035, + PID_PARTICIPANT_GUID = 0x0050, + PID_PARTICIPANT_ENTITYID = 0x0051, + PID_GROUP_GUID = 0x0052, + PID_GROUP_ENTITYID = 0x0053, + PID_BUILTIN_ENDPOINT_SET = 0x0058, + PID_PROPERTY_LIST = 0x0059, + PID_ENDPOINT_GUID = 0x005a, + PID_TYPE_MAX_SIZE_SERIALIZED = 0x0060, + PID_ENTITY_NAME = 0x0062, + PID_KEY_HASH = 0x0070, + PID_STATUS_INFO = 0x0071 +}; + +enum BuildInEndpointSet : uint32_t { + DISC_BIE_PARTICIPANT_ANNOUNCER = 1 << 0, + DISC_BIE_PARTICIPANT_DETECTOR = 1 << 1, + DISC_BIE_PUBLICATION_ANNOUNCER = 1 << 2, + DISC_BIE_PUBLICATION_DETECTOR = 1 << 3, + DISC_BIE_SUBSCRIPTION_ANNOUNCER = 1 << 4, + DISC_BIE_SUBSCRIPTION_DETECTOR = 1 << 5, + + DISC_BIE_PARTICIPANT_PROXY_ANNOUNCER = 1 << 6, + DISC_BIE_PARTICIPANT_PROXY_DETECTOR = 1 << 7, + DISC_BIE_PARTICIPANT_STATE_ANNOUNCER = 1 << 8, + DISC_BIE_PARTICIPANT_STATE_DETECTOR = 1 << 9, + + BIE_PARTICIPANT_MESSAGE_DATA_WRITER = 1 << 10, + BIE_PARTICIPANT_MESSAGE_DATA_READER = 1 << 11, +}; + +// TODO endianess + +const std::array SCHEME_CDR_LE{0x00, 0x01}; +const std::array SCHEME_PL_CDR_LE{0x00, 0x03}; + +struct ParameterList_t { + ParameterId pid; + uint16_t length; + // Values follow +} RTPS_EMBEDDED_PACKED; +} // namespace SMElement + +enum class SubmessageKind : uint8_t { + PAD = 0x01, /* Pad */ + ACKNACK = 0x06, /* AckNack */ + HEARTBEAT = 0x07, /* Heartbeat */ + GAP = 0x08, /* Gap */ + INFO_TS = 0x09, /* InfoTimestamp */ + INFO_SRC = 0x0c, /* InfoSource */ + INFO_REPLY_IP4 = 0x0d, /* InfoReplyIp4 */ + INFO_DST = 0x0e, /* InfoDestination */ + INFO_REPLY = 0x0f, /* InfoReply */ + NACK_FRAG = 0x12, /* NackFrag */ + HEARTBEAT_FRAG = 0x13, /* HeartbeatFrag */ + DATA = 0x15, /* Data */ + DATA_FRAG = 0x16 /* DataFrag */ +}; + +enum SubMessageFlag : uint8_t { + FLAG_ENDIANESS = (1 << 0), + FLAG_BIG_ENDIAN = (0 << 0), + FLAG_LITTLE_ENDIAN = (1 << 0), + FLAG_INVALIDATE = (1 << 1), + FLAG_INLINE_QOS = (1 << 1), + FLAG_NO_PAYLOAD = (0 << 3 | 0 << 2), + FLAG_DATA_PAYLOAD = (0 << 3 | 1 << 2), + FLAG_FINAL = (1 << 1), + FLAG_HB_LIVELINESS = (1 << 2) +}; + +const std::array RTPS_PROTOCOL_NAME = {'R', 'T', 'P', 'S'}; +struct Header { + std::array protocolName; + ProtocolVersion_t protocolVersion; + VendorId_t vendorId; + GuidPrefix_t guidPrefix; + static constexpr uint16_t getRawSize() { + return sizeof(std::array) + sizeof(ProtocolVersion_t) + + sizeof(VendorId_t) + sizeof(GuidPrefix_t); + } +}; + +struct SubmessageHeader { + SubmessageKind submessageId; + uint8_t flags; + uint16_t octetsToNextHeader; + static constexpr uint16_t getRawSize() { + return sizeof(SubmessageKind) + sizeof(uint8_t) + sizeof(uint16_t); + } + + bool finalFlag() const { return (flags & (SubMessageFlag::FLAG_FINAL)); } +}; + +struct SubmessageData { + SubmessageHeader header; + uint16_t extraFlags; + uint16_t octetsToInlineQos; + EntityId_t readerId; + EntityId_t writerId; + SequenceNumber_t writerSN; + static constexpr uint16_t getRawSize() { + return SubmessageHeader::getRawSize() + sizeof(uint16_t) + + sizeof(uint16_t) + (2 * 3 + 2 * 1) // EntityID + + sizeof(SequenceNumber_t); + } +}; + +struct SubmessageHeartbeat { + SubmessageHeader header; + EntityId_t readerId; + EntityId_t writerId; + SequenceNumber_t firstSN; + SequenceNumber_t lastSN; + Count_t count; + static constexpr uint16_t getRawSize() { + return SubmessageHeader::getRawSize() + (2 * 3 + 2 * 1) // EntityID + + 2 * sizeof(SequenceNumber_t) + sizeof(Count_t); + } +}; + +struct SubmessageInfoDST { + SubmessageHeader header; + GuidPrefix_t guidPrefix; + + static constexpr uint16_t getRawSize() { + return SubmessageHeader::getRawSize() + (sizeof(guidPrefix)); + } +}; + +struct SubmessageGap { + SubmessageHeader header; + EntityId_t readerId; + EntityId_t writerId; + SequenceNumber_t gapStart; + SequenceNumberSet gapList; + + static uint16_t getRawSizeWithoutSNSet() { + return SubmessageHeader::getRawSize() + + (2 * (3 + 1) + 8); // 2*EntityID + GapStart + } + + static uint16_t getRawSizeWithSingleElementSNSet() { + return SubmessageHeader::getRawSize() + + (2 * (3 + 1) + 8 + 8 + + 4); // 2*EntityID + GapStart + bitmapBase + numBits + } +}; + +struct SubmessageAckNack { + SubmessageHeader header; + EntityId_t readerId; + EntityId_t writerId; + SequenceNumberSet readerSNState; + Count_t count; + static uint16_t getRawSize(const SequenceNumberSet &set) { + uint16_t bitMapSize = 0; + if (set.numBits != 0) { + bitMapSize = static_cast(4 * (((set.numBits - 1) / 32) + 1)); + } + return getRawSizeWithoutSNSet() + sizeof(SequenceNumber_t) + + sizeof(uint32_t) + bitMapSize; // SequenceNumberSet + } + static uint16_t getRawSizeWithoutSNSet() { + return SubmessageHeader::getRawSize() + (2 * (3 + 1)) + sizeof(Count_t); + } +}; + +template +bool serializeMessage(Buffer &buffer, Header &header) { + if (!buffer.reserve(Header::getRawSize())) { + return false; + } + + buffer.append(header.protocolName.data(), sizeof(std::array)); + buffer.append(reinterpret_cast(&header.protocolVersion), + sizeof(ProtocolVersion_t)); + buffer.append(header.vendorId.vendorId.data(), sizeof(VendorId_t)); + buffer.append(header.guidPrefix.id.data(), sizeof(GuidPrefix_t)); + return true; +} + +template +bool serializeMessage(Buffer &buffer, SubmessageHeader &header) { + if (!buffer.reserve(Header::getRawSize())) { + return false; + } + buffer.reserve(SubmessageHeader::getRawSize()); + + buffer.append(reinterpret_cast(&header.submessageId), + sizeof(SubmessageKind)); + buffer.append(&header.flags, sizeof(uint8_t)); + buffer.append(reinterpret_cast(&header.octetsToNextHeader), + sizeof(uint16_t)); + return true; +} + +template +bool serializeMessage(Buffer &buffer, SubmessageInfoDST &msg) { + if (!buffer.reserve(SubmessageInfoDST::getRawSize())) { + return false; + } + + bool ret = serializeMessage(buffer, msg.header); + if (!ret) { + return false; + } + + return buffer.append(msg.guidPrefix.id.data(), sizeof(GuidPrefix_t)); +} + +template +bool serializeMessage(Buffer &buffer, SubmessageData &msg) { + if (!buffer.reserve(SubmessageData::getRawSize())) { + return false; + } + + serializeMessage(buffer, msg.header); + + buffer.append(reinterpret_cast(&msg.extraFlags), sizeof(uint16_t)); + buffer.append(reinterpret_cast(&msg.octetsToInlineQos), + sizeof(uint16_t)); + buffer.append(msg.readerId.entityKey.data(), msg.readerId.entityKey.size()); + buffer.append(reinterpret_cast(&msg.readerId.entityKind), + sizeof(EntityKind_t)); + buffer.append(msg.writerId.entityKey.data(), msg.writerId.entityKey.size()); + buffer.append(reinterpret_cast(&msg.writerId.entityKind), + sizeof(EntityKind_t)); + buffer.append(reinterpret_cast(&msg.writerSN.high), + sizeof(msg.writerSN.high)); + buffer.append(reinterpret_cast(&msg.writerSN.low), + sizeof(msg.writerSN.low)); + return true; +} + +template +bool serializeMessage(Buffer &buffer, SubmessageHeartbeat &msg) { + if (!buffer.reserve(SubmessageHeartbeat::getRawSize())) { + return false; + } + + serializeMessage(buffer, msg.header); + + buffer.append(msg.readerId.entityKey.data(), msg.readerId.entityKey.size()); + buffer.append(reinterpret_cast(&msg.readerId.entityKind), + sizeof(EntityKind_t)); + buffer.append(msg.writerId.entityKey.data(), msg.writerId.entityKey.size()); + buffer.append(reinterpret_cast(&msg.writerId.entityKind), + sizeof(EntityKind_t)); + buffer.append(reinterpret_cast(&msg.firstSN.high), + sizeof(msg.firstSN.high)); + buffer.append(reinterpret_cast(&msg.firstSN.low), + sizeof(msg.firstSN.low)); + buffer.append(reinterpret_cast(&msg.lastSN.high), + sizeof(msg.lastSN.high)); + buffer.append(reinterpret_cast(&msg.lastSN.low), + sizeof(msg.lastSN.low)); + buffer.append(reinterpret_cast(&msg.count.value), + sizeof(msg.count.value)); + return true; +} + +template +bool serializeMessage(Buffer &buffer, SubmessageAckNack &msg) { + if (!buffer.reserve(SubmessageAckNack::getRawSize(msg.readerSNState))) { + return false; + } + + serializeMessage(buffer, msg.header); + + buffer.append(msg.readerId.entityKey.data(), msg.readerId.entityKey.size()); + buffer.append(reinterpret_cast(&msg.readerId.entityKind), + sizeof(EntityKind_t)); + buffer.append(msg.writerId.entityKey.data(), msg.writerId.entityKey.size()); + buffer.append(reinterpret_cast(&msg.writerId.entityKind), + sizeof(EntityKind_t)); + buffer.append(reinterpret_cast(&msg.readerSNState.base.high), + sizeof(msg.readerSNState.base.high)); + buffer.append(reinterpret_cast(&msg.readerSNState.base.low), + sizeof(msg.readerSNState.base.low)); + buffer.append(reinterpret_cast(&msg.readerSNState.numBits), + sizeof(uint32_t)); + if (msg.readerSNState.numBits != 0) { + buffer.append(reinterpret_cast(msg.readerSNState.bitMap.data()), + 4 * ((msg.readerSNState.numBits / 32) + 1)); + } + buffer.append(reinterpret_cast(&msg.count.value), + sizeof(msg.count.value)); + return true; +} + +template +bool serializeMessage(Buffer &buffer, SubmessageGap &msg) { + if (msg.gapList.numBits != 0) { + return false; + } + if (!buffer.reserve(36)) { + return false; + } + + serializeMessage(buffer, msg.header); + + buffer.append(msg.readerId.entityKey.data(), msg.readerId.entityKey.size()); + buffer.append(reinterpret_cast(&msg.readerId.entityKind), + sizeof(EntityKind_t)); + buffer.append(msg.writerId.entityKey.data(), msg.writerId.entityKey.size()); + buffer.append(reinterpret_cast(&msg.writerId.entityKind), + sizeof(EntityKind_t)); + + buffer.append(reinterpret_cast(&msg.gapStart.high), + sizeof(msg.gapStart.high)); + buffer.append(reinterpret_cast(&msg.gapStart.low), + sizeof(msg.gapStart.low)); + + buffer.append(reinterpret_cast(&msg.gapList.base.high), + sizeof(msg.gapList.base.high)); + buffer.append(reinterpret_cast(&msg.gapList.base.low), + sizeof(msg.gapList.base.low)); + + buffer.append(reinterpret_cast(&msg.gapList.numBits), + sizeof(uint32_t)); + + return true; +} + +struct MessageProcessingInfo { + MessageProcessingInfo(const uint8_t *data, DataSize_t size) + : data(data), size(size) {} + const uint8_t *data; + const DataSize_t size; + + //! Offset to the next unprocessed byte + DataSize_t nextPos = 0; + + inline const uint8_t *getPointerToCurrentPos() const { + return &data[nextPos]; + } + + //! Returns the size of data which isn't processed yet + inline DataSize_t getRemainingSize() const { return size - nextPos; } +}; + +bool deserializeMessage(const MessageProcessingInfo &info, Header &header); + +bool deserializeMessage(const MessageProcessingInfo &info, + SubmessageHeader &header); + +bool deserializeMessage(const MessageProcessingInfo &info, SubmessageData &msg); + +bool deserializeMessage(const MessageProcessingInfo &info, + SubmessageHeartbeat &msg); + +bool deserializeMessage(const MessageProcessingInfo &info, + SubmessageAckNack &msg); + +bool deserializeMessage(const MessageProcessingInfo &info, SubmessageGap &msg); + +void deserializeSNS(const uint8_t *&position, SequenceNumberSet &set, + std::size_t num_bitfields); + +} // namespace rtps + +#if defined(_MSC_VER) +#pragma pack(pop) +#endif +#undef RTPS_EMBEDDED_PACKED + +#endif // RTPS_MESSAGES_H diff --git a/components/rtps_embedded/include/rtps/rtps.h b/components/rtps_embedded/include/rtps/rtps.h new file mode 100644 index 000000000..4415a3377 --- /dev/null +++ b/components/rtps_embedded/include/rtps/rtps.h @@ -0,0 +1,34 @@ +/* +The MIT License +Copyright (c) 2019 Lehrstuhl Informatik 11 - RWTH Aachen University +Modifications Copyright (c) 2026 ATDev +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: +The above copyright notice and this permission notice shall be included in +all copies or substantial portions of the Software. +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +THE SOFTWARE + +This file is part of embeddedRTPS. + +Author: i11 - Embedded Software, RWTH Aachen University +*/ + +#ifndef RTPS_RTPS_H +#define RTPS_RTPS_H + +#include "rtps/entities/Domain.h" + +namespace rtps { +} // namespace rtps + +#endif // RTPS_RTPS_H diff --git a/components/rtps_embedded/include/rtps/storages/CacheChange.h b/components/rtps_embedded/include/rtps/storages/CacheChange.h new file mode 100644 index 000000000..a053b6d1e --- /dev/null +++ b/components/rtps_embedded/include/rtps/storages/CacheChange.h @@ -0,0 +1,73 @@ +/* +The MIT License +Copyright (c) 2019 Lehrstuhl Informatik 11 - RWTH Aachen University +Modifications Copyright (c) 2026 ATDev +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: +The above copyright notice and this permission notice shall be included in +all copies or substantial portions of the Software. +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +THE SOFTWARE + +This file is part of embeddedRTPS. + +Author: i11 - Embedded Software, RWTH Aachen University +*/ + +#ifndef PROJECT_CACHECHANGE_H +#define PROJECT_CACHECHANGE_H + +#include "rtps/common/types.h" +#include "rtps/storages/PayloadBuffer.h" + +#include + +namespace rtps { +struct CacheChange { + using TimePoint = std::chrono::steady_clock::time_point; + + ChangeKind_t kind = ChangeKind_t::INVALID; + bool inLineQoS = false; + bool disposeAfterWrite = false; + TimePoint sentTime{}; + SequenceNumber_t sequenceNumber = SEQUENCENUMBER_UNKNOWN; + PayloadBuffer data; + + CacheChange &operator=(const CacheChange &other) = delete; + + CacheChange &operator=(CacheChange &&other) noexcept { + kind = other.kind; + inLineQoS = other.inLineQoS; + disposeAfterWrite = other.disposeAfterWrite; + sentTime = other.sentTime; + sequenceNumber = other.sequenceNumber; + data = std::move(other.data); + return *this; + } + + CacheChange() = default; + CacheChange(ChangeKind_t kind, SequenceNumber_t sequenceNumber) + : kind(kind), sequenceNumber(sequenceNumber){}; + + void reset() { + kind = ChangeKind_t::INVALID; + sequenceNumber = SEQUENCENUMBER_UNKNOWN; + inLineQoS = false; + disposeAfterWrite = false; + sentTime = TimePoint{}; + } + + bool isInitialized() { return (kind != ChangeKind_t::INVALID); } +}; +} // namespace rtps + +#endif // PROJECT_CACHECHANGE_H diff --git a/components/rtps_embedded/include/rtps/storages/HistoryCacheWithDeletion.h b/components/rtps_embedded/include/rtps/storages/HistoryCacheWithDeletion.h new file mode 100644 index 000000000..4dc213004 --- /dev/null +++ b/components/rtps_embedded/include/rtps/storages/HistoryCacheWithDeletion.h @@ -0,0 +1,283 @@ +/* +The MIT License +Copyright (c) 2019 Lehrstuhl Informatik 11 - RWTH Aachen University +Modifications Copyright (c) 2026 ATDev +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: +The above copyright notice and this permission notice shall be included in +all copies or substantial portions of the Software. +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +THE SOFTWARE + +This file is part of embeddedRTPS. + +Author: i11 - Embedded Software, RWTH Aachen University +*/ + +#ifndef HISTORYCACHEWITHDELETION_H +#define HISTORYCACHEWITHDELETION_H + +#include +#include + +namespace rtps { + +/** + * Extension of the SimpleHistoryCache that allows for deletion operation at the + * cost of efficieny + * TODO: Replace with something better in the future! + * Likely only used for SEDP + */ +template class HistoryCacheWithDeletion { +public: + HistoryCacheWithDeletion() = default; + + uint32_t m_dispose_after_write_cnt = 0; + + bool isFull() const { + uint16_t it = m_head; + incrementIterator(it); + return it == m_tail; + } + + const CacheChange *addChange(const uint8_t *data, DataSize_t size, + bool inLineQoS, bool disposeAfterWrite) { + CacheChange change; + change.kind = ChangeKind_t::ALIVE; + change.inLineQoS = inLineQoS; + change.disposeAfterWrite = disposeAfterWrite; + change.data.reserve(size); + change.data.append(data, size); + change.sequenceNumber = ++m_lastUsedSequenceNumber; + + if (disposeAfterWrite) { + m_dispose_after_write_cnt++; + } + + CacheChange *place = &m_buffer[m_head]; + incrementHead(); + + *place = std::move(change); + return place; + } + + const CacheChange *addChange(const uint8_t *data, DataSize_t size) { + return addChange(data, size, 0, false); + } + + void removeUntilIncl(SequenceNumber_t sn) { + if (m_head == m_tail) { + return; + } + + if (getCurrentSeqNumMax() <= sn) { // We won't overrun head + m_head = m_tail; + return; + } + + while (m_buffer[m_tail].sequenceNumber <= sn) { + incrementTail(); + } + } + + void dropOldest() { removeUntilIncl(getCurrentSeqNumMin()); } + + bool dropChange(const SequenceNumber_t &sn) { + uint16_t idx_to_clear; + CacheChange *change; + if (!getChangeBySN(sn, &change, idx_to_clear)) { + return false; // sn does not exist, nothing to do + } + + if (idx_to_clear == m_tail) { + m_buffer[m_tail].reset(); + incrementTail(); + return true; + } + + uint16_t prev = idx_to_clear; + do { + prev = idx_to_clear - 1; + if (prev >= m_buffer.size()) { + prev = m_buffer.size() - 1; + } + + m_buffer[idx_to_clear] = std::move(m_buffer[prev]); + idx_to_clear = prev; + + } while (prev != m_tail); + + incrementTail(); + + return true; + } + + bool setCacheChangeKind(const SequenceNumber_t &sn, ChangeKind_t kind) { + CacheChange *change = getChangeBySN(sn); + if (change == nullptr) { + return false; + } + + change->kind = kind; + return true; + } + + CacheChange *getChangeBySN(SequenceNumber_t sn) { + CacheChange *change; + uint16_t position; + if (getChangeBySN(sn, &change, position)) { + return change; + } else { + return nullptr; + } + } + + bool isEmpty() { return (m_head == m_tail); } + + const SequenceNumber_t &getCurrentSeqNumMin() const { + if (m_head == m_tail) { + return SEQUENCENUMBER_UNKNOWN; + } else { + return m_buffer[m_tail].sequenceNumber; + } + } + + const SequenceNumber_t &getCurrentSeqNumMax() const { + if (m_head == m_tail) { + return SEQUENCENUMBER_UNKNOWN; + } else { + return m_lastUsedSequenceNumber; + } + } + + const SequenceNumber_t &getLastUsedSequenceNumber() { + return m_lastUsedSequenceNumber; + } + + void clear() { + m_head = 0; + m_tail = 0; + m_lastUsedSequenceNumber = {0, 0}; + } +#ifdef DEBUG_HISTORY_CACHE_WITH_DELETION + void print() { + for (unsigned int i = 0; i < m_buffer.size(); i++) { + std::cout << "[" << i << "] " + << " SN = " << m_buffer[i].sequenceNumber.low; + switch (m_buffer[i].kind) { + case ChangeKind_t::ALIVE: + std::cout << " Type = ALIVE"; + break; + case ChangeKind_t::INVALID: + std::cout << " Type = INVALID"; + break; + case ChangeKind_t::NOT_ALIVE_DISPOSED: + std::cout << " Type = DISPOSED"; + break; + } + if (m_head == i) { + std::cout << " <- HEAD"; + } + if (m_tail == i) { + std::cout << " <- TAIL"; + } + std::cout << std::endl; + } + } +#endif + bool isSNInRange(const SequenceNumber_t &sn) { + if (isEmpty()) { + return false; + } + SequenceNumber_t minSN = getCurrentSeqNumMin(); + if (sn < minSN || getCurrentSeqNumMax() < sn) { + return false; + } + return true; + } + +private: + std::array m_buffer{}; + uint16_t m_head = 0; + uint16_t m_tail = 0; + static_assert(sizeof(SIZE) <= sizeof(m_head), + "Iterator is large enough for given size"); + + SequenceNumber_t m_lastUsedSequenceNumber{0, 0}; + + bool getChangeBySN(const SequenceNumber_t &sn, CacheChange **out_change, + uint16_t &out_buffer_position) { + if (!isSNInRange(sn)) { + return false; + } + static_assert(std::is_unsigned::value, + "Underflow well defined"); + static_assert(sizeof(m_tail) <= sizeof(uint16_t), "Cast ist well defined"); + + unsigned int cur_idx = m_tail; + while (cur_idx != m_head) { + if (m_buffer[cur_idx].sequenceNumber == sn) { + *out_change = &m_buffer[cur_idx]; + out_buffer_position = cur_idx; + return true; + } + // Sequence numbers are consecutive + if (m_buffer[cur_idx].sequenceNumber > sn) { + *out_change = nullptr; + return false; + } + + cur_idx++; + if (cur_idx >= m_buffer.size()) { + cur_idx -= m_buffer.size(); + } + } + + *out_change = nullptr; + return false; + } + + inline void incrementHead() { + incrementIterator(m_head); + if (m_head == m_tail) { + // Move without check + incrementIterator(m_tail); // drop one + } + } + + inline void incrementIterator(uint16_t &iterator) const { + ++iterator; + if (iterator >= m_buffer.size()) { + iterator = 0; + } + } + + inline void incrementTail() { + if (m_buffer[m_tail].disposeAfterWrite) { + m_dispose_after_write_cnt--; + } + if (m_head != m_tail) { + m_buffer[m_tail].reset(); + incrementIterator(m_tail); + } + } + +protected: + // This constructor was created for unit testing + explicit HistoryCacheWithDeletion(SequenceNumber_t lastUsed) + : HistoryCacheWithDeletion() { + m_lastUsedSequenceNumber = lastUsed; + } +}; +} // namespace rtps + +#endif // HISTORYCACHEWITHDELETION_H diff --git a/components/rtps_embedded/include/rtps/storages/MemoryPool.h b/components/rtps_embedded/include/rtps/storages/MemoryPool.h new file mode 100644 index 000000000..cb8161062 --- /dev/null +++ b/components/rtps_embedded/include/rtps/storages/MemoryPool.h @@ -0,0 +1,193 @@ +/* +The MIT License +Copyright (c) 2019 Lehrstuhl Informatik 11 - RWTH Aachen University +Modifications Copyright (c) 2026 ATDev +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: +The above copyright notice and this permission notice shall be included in +all copies or substantial portions of the Software. +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +THE SOFTWARE + +This file is part of embeddedRTPS. + +Author: i11 - Embedded Software, RWTH Aachen University +*/ + +#ifndef RTPS_MEMORYPOOL_H +#define RTPS_MEMORYPOOL_H + +#include +#include +#include + +namespace rtps { + +template class MemoryPool { +public: + template class MemoryPoolIterator { + public: + using iterator_category = std::input_iterator_tag; + using value_type = IT_TYPE; + using difference_type = uint8_t; + using pointer = IT_TYPE *; + using reference = IT_TYPE &; + + explicit MemoryPoolIterator(MemoryPool &pool) : m_pool(pool) { + memcpy(m_bitMap, m_pool.m_bitMap, sizeof(m_bitMap)); + } + + // bool operator==(const MemoryPoolIterator& other) const{ + // return bit == other.bit; + //} + + bool operator!=(const MemoryPoolIterator &other) const { + return m_bit != other.m_bit; + } + + reference operator*() const { return m_pool.m_data[m_bit]; } + + reference operator->() const { return m_pool.m_data[m_bit]; } + + // Pre-increment + MemoryPoolIterator &operator++() { + if (m_pool.m_numElements == 0) { + m_bit = SIZE; + return *this; + } + uint32_t bucket; + do { + ++m_bit; + bucket = m_bit / static_cast(8); + } while (!(m_bitMap[bucket] & (1 << (m_bit % 8))) && m_bit < SIZE); + + return *this; + } + + // Post-increment + MemoryPoolIterator operator++(int) { + MemoryPoolIterator tmp(*this); + ++(*this); + return tmp; + } + + private: + friend class MemoryPool; + MemoryPool &m_pool; + uint8_t m_bitMap[SIZE / 8 + 1]; + uint32_t m_bit = 0; + }; + + typedef MemoryPoolIterator MemPoolIter; + typedef MemoryPoolIterator const_MemPoolIter; + + typedef bool (*condition_fp)(TYPE); + + uint32_t getSize() { return SIZE; } + + bool isFull() { return m_numElements == SIZE; } + + bool isEmpty() { return m_numElements == 0; } + + uint32_t getNumElements() { return m_numElements; } + + bool add(const TYPE &data) { + if (isFull()) { + printf("[MemoryPool] RESSOURCE LIMIT EXCEEDED \n"); + return false; + } + for (uint32_t bucket = 0; bucket < sizeof(m_bitMap); ++bucket) { + if (m_bitMap[bucket] != 0xFF) { + uint8_t byte = m_bitMap[bucket]; + for (uint8_t bit = 0; bit < 8; ++bit) { + if (!(byte & 1)) { + m_bitMap[bucket] |= 1 << bit; + m_data[bucket * 8 + bit] = data; + ++m_numElements; + return true; + } + byte = byte >> 1; + } + } + } + return false; + } + + /** + * Parameters are used in that way to allow lambdas with captures. Use this by + * creating two: E.g.: auto callback=[data](TYPE& value){return value == + * data;}; auto thunk=[](void* arg, TYPE& value){return + * (*static_cast(arg))(value);}; + * + * and then simply call: + * remove(thunk, &callback) + * + * NOTE: You have to make sure that the callback did not run out of scope. + */ + bool remove(bool (*jumppad)(void *, const TYPE &data), + void *isCorrectElement) { + bool retcode = false; + for (auto it = begin(); it != end(); ++it) { + if (jumppad(isCorrectElement, *it)) { + const uint32_t bucket = it.m_bit / uint32_t{8}; + const uint32_t pos = + it.m_bit & + uint32_t{ + 7}; // 7 sets all bits above and including the one for 8 to 0 + m_bitMap[bucket] &= ~(static_cast(1) << pos); + --m_numElements; + retcode = true; + } + } + return retcode; + } + + void clear() { + for (unsigned int i = 0; i < (SIZE / 8 + 1); i++) { + m_bitMap[i] = 0; + } + m_numElements = 0; + } + + TYPE *find(bool (*jumppad)(void *, const TYPE &data), + void *isCorrectElement) { + for (auto it = begin(); it != end(); ++it) { + if (jumppad(isCorrectElement, *it)) { + return &(*it); + } + } + return nullptr; + } + + MemPoolIter begin() { + MemPoolIter it(*this); + if (!(m_bitMap[0] & 1)) { + ++it; + } + return it; + } + + MemPoolIter end() { + MemPoolIter endIt(*this); + endIt.m_bit = SIZE; + return endIt; + } + +private: + uint8_t m_bitMap[SIZE / 8 + 1]{}; + uint32_t m_numElements = 0; + TYPE m_data[SIZE]; +}; + +} // namespace rtps + +#endif // RTPS_MEMORYPOOL_H diff --git a/components/rtps_embedded/include/rtps/storages/PayloadBuffer.h b/components/rtps_embedded/include/rtps/storages/PayloadBuffer.h new file mode 100644 index 000000000..f2b3de123 --- /dev/null +++ b/components/rtps_embedded/include/rtps/storages/PayloadBuffer.h @@ -0,0 +1,71 @@ +/* +The MIT License +Copyright (c) 2019 Lehrstuhl Informatik 11 - RWTH Aachen University +Modifications Copyright (c) 2026 ATDev +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: +The above copyright notice and this permission notice shall be included in +all copies or substantial portions of the Software. +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +THE SOFTWARE + +This file is part of embeddedRTPS. + +Author: i11 - Embedded Software, RWTH Aachen University +*/ + +#ifndef RTPS_PAYLOADBUFFER_H +#define RTPS_PAYLOADBUFFER_H + +#include + +#include "rtps/common/types.h" + +namespace rtps { + +struct PayloadBuffer { + std::vector bytes; + + bool isValid() const { return true; } + + bool reserve(DataSize_t length) { + if (length > bytes.max_size() - bytes.size()) { + return false; + } + bytes.reserve(bytes.size() + length); + return true; + } + + bool append(const uint8_t *data, DataSize_t length) { + if (length == 0) { + return true; + } + if (data == nullptr) { + return false; + } + + bytes.insert(bytes.end(), data, data + length); + return true; + } + + void append(const PayloadBuffer &other) { + bytes.insert(bytes.end(), other.bytes.begin(), other.bytes.end()); + } + + DataSize_t spaceUsed() const { return static_cast(bytes.size()); } + + void reset() { bytes.clear(); } +}; + +} // namespace rtps + +#endif // RTPS_PAYLOADBUFFER_H diff --git a/components/rtps_embedded/include/rtps/storages/SimpleHistoryCache.h b/components/rtps_embedded/include/rtps/storages/SimpleHistoryCache.h new file mode 100644 index 000000000..666966c2b --- /dev/null +++ b/components/rtps_embedded/include/rtps/storages/SimpleHistoryCache.h @@ -0,0 +1,179 @@ +/* +The MIT License +Copyright (c) 2019 Lehrstuhl Informatik 11 - RWTH Aachen University +Modifications Copyright (c) 2026 ATDev +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: +The above copyright notice and this permission notice shall be included in +all copies or substantial portions of the Software. +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +THE SOFTWARE + +This file is part of embeddedRTPS. + +Author: i11 - Embedded Software, RWTH Aachen University +*/ + +#ifndef PROJECT_SIMPLEHISTORYCACHE_H +#define PROJECT_SIMPLEHISTORYCACHE_H + +#include "rtps/config.h" +#include "rtps/storages/CacheChange.h" + +namespace rtps { + +/** + * Simple version of a history cache. It sets consecutive sequence numbers + * automatically which allows an easy and fast approach of dropping acknowledged + * changes. Furthermore, disposing of arbitrary changes is not possible. + * However, this is in principle easy to add by changing the ChangeKind and + * dropping it when passing it during deleting of other sequence numbers + */ +template class SimpleHistoryCache { +public: + SimpleHistoryCache() = default; + + bool isFull() const { + uint16_t it = m_head; + incrementIterator(it); + return it == m_tail; + } + + const CacheChange *addChange(const uint8_t *data, DataSize_t size, + bool inLineQoS, bool disposeAfterWrite) { + CacheChange change; + change.kind = ChangeKind_t::ALIVE; + change.inLineQoS = inLineQoS; + change.disposeAfterWrite = disposeAfterWrite; + change.data.reserve(size); + change.data.append(data, size); + change.sequenceNumber = ++m_lastUsedSequenceNumber; + + CacheChange *place = &m_buffer[m_head]; + incrementHead(); + + *place = std::move(change); + return place; + } + + const CacheChange *addChange(const uint8_t *data, DataSize_t size) { + return addChange(data, size, 0, false); + } + + void removeUntilIncl(SequenceNumber_t sn) { + if (m_head == m_tail) { + return; + } + + if (getSeqNumMax() <= sn) { // We won't overrun head + m_head = m_tail; + return; + } + + while (m_buffer[m_tail].sequenceNumber <= sn && (m_head != m_tail)) { + incrementTail(); + } + } + + void dropOldest() { removeUntilIncl(getSeqNumMin()); } + + bool setCacheChangeKind(const SequenceNumber_t &sn, ChangeKind_t kind) { + CacheChange *change = getChangeBySN(sn); + if (change == nullptr) { + return false; + } + + change->kind = kind; + return true; + } + + CacheChange *getChangeBySN(SequenceNumber_t sn) { + SequenceNumber_t minSN = getSeqNumMin(); + if (sn < minSN || getSeqNumMax() < sn) { + return nullptr; + } + static_assert(std::is_unsigned::value, + "Underflow well defined"); + static_assert(sizeof(m_tail) <= sizeof(uint16_t), "Cast ist well defined"); + // We don't overtake head, therefore difference of sn is within same range + // as iterators + uint16_t pos = m_tail + static_cast(sn.low - minSN.low); + + // Diff is smaller than the size of the array -> max one overflow + if (pos >= m_buffer.size()) { + pos -= m_buffer.size(); + } + return &m_buffer[pos]; + } + + const SequenceNumber_t &getSeqNumMin() const { + if (m_head == m_tail) { + return SEQUENCENUMBER_UNKNOWN; + } else { + return m_buffer[m_tail].sequenceNumber; + } + } + + const SequenceNumber_t &getSeqNumMax() const { + if (m_head == m_tail) { + return SEQUENCENUMBER_UNKNOWN; + } else { + return m_lastUsedSequenceNumber; + } + } + + void clear() { + m_head = 0; + m_tail = 0; + m_lastUsedSequenceNumber = {0, 0}; + } + +private: + std::array m_buffer{}; + uint16_t m_head = 0; + uint16_t m_tail = 0; + static_assert(sizeof(SIZE) <= sizeof(m_head), + "Iterator is large enough for given size"); + + SequenceNumber_t m_lastUsedSequenceNumber{0, 0}; + + inline void incrementHead() { + incrementIterator(m_head); + if (m_head == m_tail) { + // Move without check + incrementIterator(m_tail); // drop one + } + } + + inline void incrementIterator(uint16_t &iterator) const { + ++iterator; + if (iterator >= m_buffer.size()) { + iterator = 0; + } + } + + inline void incrementTail() { + if (m_head != m_tail) { + incrementIterator(m_tail); + } + } + +protected: + // This constructor was created for unit testing + explicit SimpleHistoryCache(SequenceNumber_t lastUsed) + : SimpleHistoryCache() { + m_lastUsedSequenceNumber = lastUsed; + } +}; +} // namespace rtps + +#endif // PROJECT_SIMPLEHISTORYCACHE_H diff --git a/components/rtps_embedded/include/rtps/storages/ThreadSafeCircularBuffer.h b/components/rtps_embedded/include/rtps/storages/ThreadSafeCircularBuffer.h new file mode 100644 index 000000000..c3921c13e --- /dev/null +++ b/components/rtps_embedded/include/rtps/storages/ThreadSafeCircularBuffer.h @@ -0,0 +1,80 @@ +/* +The MIT License +Copyright (c) 2019 Lehrstuhl Informatik 11 - RWTH Aachen University +Modifications Copyright (c) 2026 ATDev +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: +The above copyright notice and this permission notice shall be included in +all copies or substantial portions of the Software. +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +THE SOFTWARE + +This file is part of embeddedRTPS. + +Author: i11 - Embedded Software, RWTH Aachen University +*/ + +#ifndef RTPS_THREADSAFEQUEUE_H +#define RTPS_THREADSAFEQUEUE_H + +#include +#include +#include +#include + +namespace rtps { + +template class ThreadSafeCircularBuffer { + +public: + bool init(); + + bool moveElementIntoBuffer(T &&elem); + bool copyElementIntoBuffer(const T &elem); + + /** + * Removes the first into the given hull. Also moves responsibility for + * resources. + * @return true if element was injected. False if no element was present. + */ + bool moveFirstInto(T &hull); + bool peakFirst(T &hull); + + uint32_t numElements(); + uint32_t insertionFailures(); + + void clear(); + +private: + std::array m_buffer{}; + uint16_t m_head = 0; + uint16_t m_tail = 0; + uint32_t m_num_elements = 0; + uint32_t m_insertion_failures = 0; + static_assert(SIZE + 1 < std::numeric_limits::max(), + "Iterator is large enough for given size"); + + std::mutex m_mutex; + std::mutex m_init_mutex; + bool m_initialized = false; + + inline bool isFull(); + inline void incrementIterator(uint16_t &iterator); + inline void incrementTail(); + inline void incrementHead(); +}; + +} // namespace rtps + +#include "ThreadSafeCircularBuffer.tpp" + +#endif // RTPS_THREADSAFEQUEUE_H diff --git a/components/rtps_embedded/include/rtps/storages/ThreadSafeCircularBuffer.tpp b/components/rtps_embedded/include/rtps/storages/ThreadSafeCircularBuffer.tpp new file mode 100644 index 000000000..589963a89 --- /dev/null +++ b/components/rtps_embedded/include/rtps/storages/ThreadSafeCircularBuffer.tpp @@ -0,0 +1,152 @@ + +#ifndef RTPS_THREADSAFECIRCULARBUFFER_TPP +#define RTPS_THREADSAFECIRCULARBUFFER_TPP + +#include "rtps/utils/Log.h" + +#if TSCB_VERBOSE && RTPS_GLOBAL_VERBOSE +#define TSCB_LOG(...) \ + if (true) { \ + printf("[TSCircularBuffer] "); \ + printf(__VA_ARGS__); \ + printf("\n"); \ + } +#else +#define TSCB_LOG(...) // +#endif + +namespace rtps { + +template +bool ThreadSafeCircularBuffer::init() { + std::lock_guard init_lock(m_init_mutex); + if (m_initialized) { + return true; + } + TSCB_LOG("Using std::mutex at %p\n", static_cast(&m_mutex)); + m_initialized = true; + return true; +} + +template +bool ThreadSafeCircularBuffer::moveElementIntoBuffer(T &&elem) { + if (!init()) { + return false; + } + std::lock_guard lock(m_mutex); + if (!isFull()) { + m_buffer[m_head] = std::move(elem); + incrementHead(); + return true; + } else { + m_insertion_failures++; + return false; + } +} + +template +bool ThreadSafeCircularBuffer::copyElementIntoBuffer(const T &elem) { + if (!init()) { + return false; + } + std::lock_guard lock(m_mutex); + if (!isFull()) { + m_buffer[m_head] = elem; + incrementHead(); + return true; + } else { + m_insertion_failures++; + return false; + } +} + +template +bool ThreadSafeCircularBuffer::moveFirstInto(T &hull) { + if (!init()) { + return false; + } + std::lock_guard lock(m_mutex); + if (m_head != m_tail) { + hull = std::move(m_buffer[m_tail]); + incrementTail(); + return true; + } else { + return false; + } +} + +template +bool ThreadSafeCircularBuffer::peakFirst(T &hull) { + if (!init()) { + return false; + } + std::lock_guard lock(m_mutex); + if (m_head != m_tail) { + hull = m_buffer[m_tail]; + return true; + } else { + return false; + } +} + +template +uint32_t ThreadSafeCircularBuffer::numElements() { + if (!init()) { + return 0; + } + std::lock_guard lock(m_mutex); + return m_num_elements; +} + +template +uint32_t ThreadSafeCircularBuffer::insertionFailures() { + if (!init()) { + return 0; + } + std::lock_guard lock(m_mutex); + return m_insertion_failures; +} + +template +void ThreadSafeCircularBuffer::clear() { + if (!init()) { + return; + } + std::lock_guard lock(m_mutex); + m_head = m_tail; + m_num_elements = 0; +} + +template +bool ThreadSafeCircularBuffer::isFull() { + auto it = m_head; + incrementIterator(it); + return it == m_tail; +} + +template +inline void +ThreadSafeCircularBuffer::incrementIterator(uint16_t &iterator) { + ++iterator; + if (iterator >= m_buffer.size()) { + iterator = 0; + } +} + +template +inline void ThreadSafeCircularBuffer::incrementTail() { + incrementIterator(m_tail); + m_num_elements--; +} + +template +inline void ThreadSafeCircularBuffer::incrementHead() { + incrementIterator(m_head); + m_num_elements++; + if (m_head == m_tail) { + incrementTail(); + } +} +} // namespace rtps + +#endif // RTPS_THREADSAFECIRCULARBUFFER_TPP diff --git a/components/rtps_embedded/include/rtps/utils/Diagnostics.h b/components/rtps_embedded/include/rtps/utils/Diagnostics.h new file mode 100644 index 000000000..1cc9e9550 --- /dev/null +++ b/components/rtps_embedded/include/rtps/utils/Diagnostics.h @@ -0,0 +1,86 @@ +/* +The MIT License +Copyright (c) 2019 Lehrstuhl Informatik 11 - RWTH Aachen University +Modifications Copyright (c) 2026 ATDev +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: +The above copyright notice and this permission notice shall be included in +all copies or substantial portions of the Software. +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +THE SOFTWARE + +This file is part of embeddedRTPS. + +Author: i11 - Embedded Software, RWTH Aachen University +*/ + +#ifndef RTPS_DIAGNOSTICS_H +#define RTPS_DIAGNOSTICS_H + +#include + +namespace rtps { +namespace Diagnostics { + +namespace ThreadPool { +extern uint32_t dropped_incoming_packets_usertraffic; +extern uint32_t dropped_incoming_packets_metatraffic; + +extern uint32_t dropped_outgoing_packets_usertraffic; +extern uint32_t dropped_outgoing_packets_metatraffic; + +extern uint32_t processed_incoming_metatraffic; +extern uint32_t processed_outgoing_metatraffic; +extern uint32_t processed_incoming_usertraffic; +extern uint32_t processed_outgoing_usertraffic; + +extern uint32_t max_ever_elements_outgoing_usertraffic_queue; +extern uint32_t max_ever_elements_incoming_usertraffic_queue; + +extern uint32_t max_ever_elements_outgoing_metatraffic_queue; +extern uint32_t max_ever_elements_incoming_metatraffic_queue; +} // namespace ThreadPool + +namespace StatefulReader { +extern uint32_t sfr_unexpected_sn; +extern uint32_t sfr_retransmit_requests; +} // namespace StatefulReader + +namespace Network { +extern uint32_t lwip_allocation_failures; +} + +namespace OS { +extern uint32_t current_free_heap_size; +} + +namespace SEDP { +extern uint32_t max_ever_remote_participants; +extern uint32_t current_remote_participants; + +extern uint32_t max_ever_matched_reader_proxies; +extern uint32_t current_max_matched_reader_proxies; + +extern uint32_t max_ever_matched_writer_proxies; +extern uint32_t current_max_matched_writer_proxies; + +extern uint32_t max_ever_unmatched_reader_proxies; +extern uint32_t current_max_unmatched_reader_proxies; + +extern uint32_t max_ever_unmatched_writer_proxies; +extern uint32_t current_max_unmatched_writer_proxies; +} // namespace SEDP + +} // namespace Diagnostics +} // namespace rtps + +#endif diff --git a/components/rtps_embedded/include/rtps/utils/Log.h b/components/rtps_embedded/include/rtps/utils/Log.h new file mode 100644 index 000000000..23525c4fe --- /dev/null +++ b/components/rtps_embedded/include/rtps/utils/Log.h @@ -0,0 +1,48 @@ +/* +The MIT License +Copyright (c) 2019 Lehrstuhl Informatik 11 - RWTH Aachen University +Modifications Copyright (c) 2026 ATDev +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: +The above copyright notice and this permission notice shall be included in +all copies or substantial portions of the Software. +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +THE SOFTWARE + +This file is part of embeddedRTPS. + +Author: i11 - Embedded Software, RWTH Aachen University +*/ + +#ifndef RTPS_LOG_H +#define RTPS_LOG_H + +#include +#include + +#define RTPS_GLOBAL_VERBOSE 0 + +#define SFW_VERBOSE 1 +#define SPDP_VERBOSE 0 +#define PBUF_WRAP_VERBOSE 0 +#define SEDP_VERBOSE 1 +#define RECV_VERBOSE 0 +#define PARTICIPANT_VERBOSE 0 +#define DOMAIN_VERBOSE 1 +#define UDP_DRIVER_VERBOSE 1 +#define TSCB_VERBOSE 1 +#define SLW_VERBOSE 0 +#define SFR_VERBOSE 1 +#define SLR_VERBOSE 1 +#define THREAD_POOL_VERBOSE 1 + +#endif // RTPS_LOG_H diff --git a/components/rtps_embedded/include/rtps/utils/constants.h b/components/rtps_embedded/include/rtps/utils/constants.h new file mode 100644 index 000000000..58b5c7c74 --- /dev/null +++ b/components/rtps_embedded/include/rtps/utils/constants.h @@ -0,0 +1,29 @@ +/* +The MIT License +Copyright (c) 2019 Lehrstuhl Informatik 11 - RWTH Aachen University +Modifications Copyright (c) 2026 ATDev +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: +The above copyright notice and this permission notice shall be included in +all copies or substantial portions of the Software. +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +THE SOFTWARE + +This file is part of embeddedRTPS. + +Author: i11 - Embedded Software, RWTH Aachen University +*/ + +#ifndef RTPS_CONSTANTS_H +#define RTPS_CONSTANTS_H + +#endif // RTPS_CONSTANTS_H diff --git a/components/rtps_embedded/include/rtps/utils/hash.h b/components/rtps_embedded/include/rtps/utils/hash.h new file mode 100644 index 000000000..2cd6ae5e6 --- /dev/null +++ b/components/rtps_embedded/include/rtps/utils/hash.h @@ -0,0 +1,42 @@ +/* +The MIT License +Copyright (c) 2019 Lehrstuhl Informatik 11 - RWTH Aachen University +Modifications Copyright (c) 2026 ATDev +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: +The above copyright notice and this permission notice shall be included in +all copies or substantial portions of the Software. +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +THE SOFTWARE + +This file is part of embeddedRTPS. + +Author: i11 - Embedded Software, RWTH Aachen University +*/ + +#ifndef RTPS_HASH_H +#define RTPS_HASH_H + +#include + +namespace rtps { +inline size_t hashCharArray(const char *p, size_t s) { + size_t result = 0; + const size_t prime = 31; + for (size_t i = 0; i < s; ++i) { + result = p[i] + (result * prime); + } + return result; +} +} // namespace rtps + +#endif diff --git a/components/rtps_embedded/include/rtps/utils/printutils.h b/components/rtps_embedded/include/rtps/utils/printutils.h new file mode 100644 index 000000000..3fc599344 --- /dev/null +++ b/components/rtps_embedded/include/rtps/utils/printutils.h @@ -0,0 +1,31 @@ +// +// Created by andreas on 13.01.19. +// + +#ifndef RTPS_PRINTUTILS_H +#define RTPS_PRINTUTILS_H + +#include "rtps/common/types.h" + +inline void printEntityId(rtps::EntityId_t id) { + for (const auto byte : id.entityKey) { + printf("%x", (int)byte); + } + printf("%x", static_cast(id.entityKind)); + printf("\n"); + +} + +inline void printGuidPrefix(rtps::GuidPrefix_t prefix) { + for (const auto byte : prefix.id) { + printf("%x", (int)byte); + } +} + +inline void printGuid(rtps::Guid_t guid) { + printGuidPrefix(guid.prefix); + printf(":"); + printEntityId(guid.entityId); +} + +#endif // RTPS_PRINTUTILS_H diff --git a/components/rtps_embedded/include/rtps/utils/sysFunctions.h b/components/rtps_embedded/include/rtps/utils/sysFunctions.h new file mode 100644 index 000000000..028568e6b --- /dev/null +++ b/components/rtps_embedded/include/rtps/utils/sysFunctions.h @@ -0,0 +1,48 @@ +/* +The MIT License +Copyright (c) 2019 Lehrstuhl Informatik 11 - RWTH Aachen University +Modifications Copyright (c) 2026 ATDev +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: +The above copyright notice and this permission notice shall be included in +all copies or substantial portions of the Software. +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +THE SOFTWARE + +This file is part of embeddedRTPS. + +Author: i11 - Embedded Software, RWTH Aachen University +*/ + +#ifndef PROJECT_SYSFUNCTIONS_H +#define PROJECT_SYSFUNCTIONS_H + +#include "rtps/common/types.h" + +#include + +namespace rtps { +inline Time_t getCurrentTimeStamp() { + static const auto start = std::chrono::steady_clock::now(); + const auto elapsed = std::chrono::steady_clock::now() - start; + const auto nowMs = + std::chrono::duration_cast(elapsed).count(); + + Time_t now; + now.seconds = static_cast(nowMs / 1000); + now.fraction = static_cast((nowMs % 1000) * + ((1ULL << 32) / 1000)); + return now; +} +} // namespace rtps + +#endif // PROJECT_SYSFUNCTIONS_H diff --git a/components/rtps_embedded/include/rtps/utils/udpUtils.h b/components/rtps_embedded/include/rtps/utils/udpUtils.h new file mode 100644 index 000000000..f32ba16a9 --- /dev/null +++ b/components/rtps_embedded/include/rtps/utils/udpUtils.h @@ -0,0 +1,121 @@ +/* +The MIT License +Copyright (c) 2019 Lehrstuhl Informatik 11 - RWTH Aachen University +Modifications Copyright (c) 2026 ATDev +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: +The above copyright notice and this permission notice shall be included in +all copies or substantial portions of the Software. +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +THE SOFTWARE + +This file is part of embeddedRTPS. + +Author: i11 - Embedded Software, RWTH Aachen University +*/ + +#ifndef RTPS_UDP_UTILS_H +#define RTPS_UDP_UTILS_H + +#include "rtps/config.h" + +#include + +namespace rtps { +namespace { +const uint16_t PB = 7400; // Port Base Number +const uint16_t DG = 250; // DomainId Gain +const uint16_t PG = 2; // ParticipantId Gain +// Additional Offsets +const uint16_t D0 = 0; // Builtin multicast +const uint16_t D1 = 10; // Builtin unicast +const uint16_t D2 = 1; // User multicast +const uint16_t D3 = 11; // User unicast +} // namespace + +constexpr std::array +transformIP4ToU32(uint8_t MSB, uint8_t p2, uint8_t p1, uint8_t LSB) { + return {MSB, p2, p1, LSB}; +} + +constexpr Ip4Port_t getBuiltInUnicastPort(ParticipantId_t participantId) { + return PB + DG * Config::DOMAIN_ID + D1 + PG * participantId; +} + +constexpr Ip4Port_t getBuiltInMulticastPort() { + return PB + DG * Config::DOMAIN_ID + D0; +} + +constexpr Ip4Port_t getUserUnicastPort(ParticipantId_t participantId) { + return PB + DG * Config::DOMAIN_ID + D3 + PG * participantId; +} + +constexpr Ip4Port_t getUserMulticastPort() { + return PB + DG * Config::DOMAIN_ID + D2; +} + +constexpr bool isUserPort(Ip4Port_t port) { + return (port & 1) == 1; +} // really useful? There may be other user ports than just odd ones? + +inline bool isMultiCastPort(Ip4Port_t port) { + const auto idWithoutBase = port - PB - DG * Config::DOMAIN_ID; + return idWithoutBase == D0 || + (idWithoutBase >= D2 && + idWithoutBase < D3); // There are several UserMulticastPorts! +} + +inline bool isMetaMultiCastPort(Ip4Port_t port) { + const auto idWithoutBase = port - PB - DG * Config::DOMAIN_ID; + return idWithoutBase == D0; +} + +inline bool isUserMultiCastPort(Ip4Port_t port) { + const auto idWithoutBase = port - PB - DG * Config::DOMAIN_ID; + return (idWithoutBase >= D2 && idWithoutBase < D1); +} + +inline bool isZeroAddress(const std::array &address) { + return address[0] == 0 && address[1] == 0 && address[2] == 0 && + address[3] == 0; +} + +inline bool isMulticastAddress(const std::array &address) { + return address[0] >= 224 && address[0] <= 239; +} + +inline ParticipantId_t getParticipantIdFromUnicastPort(Ip4Port_t port, + bool isUserPort) { + + const auto basePart = PB + DG * Config::DOMAIN_ID; + ParticipantId_t participantPart = port - basePart; + + uint16_t offset = 0; + if (isUserPort) { + offset = D3; + } else { + offset = D1; + } + + participantPart -= offset; + + auto id = static_cast(participantPart / PG); + if (id * PG + basePart + offset == port) { + return id; + } else { + return PARTICIPANT_ID_INVALID; + } +} + +} // namespace rtps + +#endif // RTPS_UDP_H diff --git a/components/rtps_embedded/src/ThreadPool.cpp b/components/rtps_embedded/src/ThreadPool.cpp new file mode 100644 index 000000000..4f4fefe7d --- /dev/null +++ b/components/rtps_embedded/src/ThreadPool.cpp @@ -0,0 +1,307 @@ +/* +The MIT License +Copyright (c) 2019 Lehrstuhl Informatik 11 - RWTH Aachen University +Modifications Copyright (c) 2026 ATDev +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: +The above copyright notice and this permission notice shall be included in +all copies or substantial portions of the Software. +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +THE SOFTWARE + +This file is part of embeddedRTPS. + +Author: i11 - Embedded Software, RWTH Aachen University +*/ + +#include "rtps/ThreadPool.h" + +#include "rtps/entities/Domain.h" +#include "rtps/entities/Writer.h" +#include "rtps/utils/Diagnostics.h" +#include "rtps/utils/Log.h" +#include "rtps/utils/udpUtils.h" +#include "thread_pool.hpp" + +using rtps::ThreadPool; + +#if THREAD_POOL_VERBOSE && RTPS_GLOBAL_VERBOSE +#include "rtps/utils/printutils.h" +#define THREAD_POOL_LOG(...) logger_.warn(__VA_ARGS__) +#else +#define THREAD_POOL_LOG(...) do { } while (0) +#endif + +ThreadPool::ThreadPool(receiveJumppad_fp receiveCallback, void *callee) + : espp::BaseComponent("RtpsThreadPool", espp::Logger::Verbosity::WARN), + m_receiveJumppad(receiveCallback), m_callee(callee) { + + if (!m_outgoingMetaTraffic.init() || !m_outgoingUserTraffic.init() || + !m_incomingMetaTraffic.init() || !m_incomingUserTraffic.init()) { + return; + } + + m_writerWorkers = std::make_unique(espp::ThreadPool::Config{ + .worker_count = Config::THREAD_POOL_NUM_WRITERS, + .max_queue_size = 0, + .auto_start = false, + .block_on_submit_when_full = false, + .worker_task_config = { + .name = "rtps_writer", + .stack_size_bytes = static_cast(Config::THREAD_POOL_WRITER_STACKSIZE), + .priority = static_cast(Config::THREAD_POOL_WRITER_PRIO), + .core_id = -1, + }, + .log_level = espp::Logger::Verbosity::WARN, + }); + + m_readerWorkers = std::make_unique(espp::ThreadPool::Config{ + .worker_count = Config::THREAD_POOL_NUM_READERS, + .max_queue_size = 0, + .auto_start = false, + .block_on_submit_when_full = false, + .worker_task_config = { + .name = "rtps_reader", + .stack_size_bytes = static_cast(Config::THREAD_POOL_READER_STACKSIZE), + .priority = static_cast(Config::THREAD_POOL_READER_PRIO), + .core_id = -1, + }, + .log_level = espp::Logger::Verbosity::WARN, + }); +} + +ThreadPool::~ThreadPool() { + if (m_running) { + stopThreads(); + } +} + +void ThreadPool::updateDiagnostics() { + + rtps::Diagnostics::ThreadPool::max_ever_elements_incoming_usertraffic_queue = + std::max(rtps::Diagnostics::ThreadPool:: + max_ever_elements_incoming_usertraffic_queue, + m_incomingUserTraffic.numElements()); + + rtps::Diagnostics::ThreadPool::max_ever_elements_outgoing_usertraffic_queue = + std::max(rtps::Diagnostics::ThreadPool:: + max_ever_elements_outgoing_usertraffic_queue, + m_outgoingUserTraffic.numElements()); + + rtps::Diagnostics::ThreadPool::max_ever_elements_incoming_metatraffic_queue = + std::max(rtps::Diagnostics::ThreadPool:: + max_ever_elements_incoming_metatraffic_queue, + m_incomingMetaTraffic.numElements()); + + rtps::Diagnostics::ThreadPool::max_ever_elements_outgoing_metatraffic_queue = + std::max(rtps::Diagnostics::ThreadPool:: + max_ever_elements_outgoing_metatraffic_queue, + m_outgoingMetaTraffic.numElements()); +} + +bool ThreadPool::startThreads() { + if (m_running) { + return true; + } + if (!m_writerWorkers || !m_readerWorkers) { + return false; + } + + m_running = true; + m_writerWorkers->start(); + m_readerWorkers->start(); + + scheduleWriterWork(); + scheduleReaderWork(); + return true; +} + +void ThreadPool::stopThreads() { + m_running = false; + if (m_writerWorkers) { + m_writerWorkers->stop(); + } + if (m_readerWorkers) { + m_readerWorkers->stop(); + } +} + +void ThreadPool::clearQueues() { + m_outgoingMetaTraffic.clear(); + m_outgoingUserTraffic.clear(); + m_incomingMetaTraffic.clear(); + m_incomingUserTraffic.clear(); +} + +bool ThreadPool::addWorkload(Writer *workload) { + bool res = false; + if (workload->isBuiltinEndpoint()) { + res = m_outgoingMetaTraffic.moveElementIntoBuffer(std::move(workload)); + } else { + res = m_outgoingUserTraffic.moveElementIntoBuffer(std::move(workload)); + } + if (!res) { + if(workload->isBuiltinEndpoint()){ + rtps::Diagnostics::ThreadPool::dropped_outgoing_packets_metatraffic++; + }else{ + rtps::Diagnostics::ThreadPool::dropped_outgoing_packets_usertraffic++; + } + THREAD_POOL_LOG("Failed to enqueue outgoing packet."); + return false; + } + + scheduleWriterWork(); + + return res; +} + +bool ThreadPool::addBuiltinPort(const Ip4Port_t &port) { + if (m_builtinPortsIdx == m_builtinPorts.size()) { + return false; + } + + // TODO: Does not allow for participant deletion! + m_builtinPorts[m_builtinPortsIdx] = port; + m_builtinPortsIdx++; + + return true; +} + +bool ThreadPool::isBuiltinPort(const Ip4Port_t &port) { + if (getBuiltInMulticastLocator().port == port) { + return true; + } + + for (unsigned int i = 0; i < m_builtinPortsIdx; i++) { + if (m_builtinPorts[i] == port) { + return true; + } + } + + return false; +} + +bool ThreadPool::addNewPacket(PacketInfo &&packet) { + bool res = false; + if (isBuiltinPort(packet.destPort)) { + res = m_incomingMetaTraffic.moveElementIntoBuffer(std::move(packet)); + } else { + res = m_incomingUserTraffic.moveElementIntoBuffer(std::move(packet)); + } + if (!res) { + THREAD_POOL_LOG("failed to enqueue packet for port {}", + static_cast(packet.destPort)); + return false; + } + + scheduleReaderWork(); + return res; +} + +void ThreadPool::scheduleWriterWork() { + if (!m_running || !m_writerWorkers) { + return; + } + + (void)m_writerWorkers->try_submit([this]() { doWriterWork(); }); +} + +void ThreadPool::doWriterWork() { + while (m_running) { + Writer *workload_usertraffic = nullptr; + bool workload_usertraffic_available = m_outgoingUserTraffic.moveFirstInto(workload_usertraffic); + if (workload_usertraffic_available) { + workload_usertraffic->progress(); + Diagnostics::ThreadPool::processed_outgoing_usertraffic++; + } + + Writer *workload_metatraffic = nullptr; + bool workload_metatraffic_available = m_outgoingMetaTraffic.moveFirstInto(workload_metatraffic); + if (workload_metatraffic_available) { + workload_metatraffic->progress(); + Diagnostics::ThreadPool::processed_outgoing_metatraffic++; + } + + if (workload_usertraffic_available || workload_metatraffic_available) { + continue; + } + + // THREAD_POOL_LOG("WriterWorker | User = %u, Meta = %u\r\n", + // static_cast(Diagnostics::ThreadPool::processed_outgoing_usertraffic), + // static_cast(Diagnostics::ThreadPool::processed_outgoing_metatraffic)); + updateDiagnostics(); + return; + } +} + +void ThreadPool::onDatagram( + void *arg, const uint8_t *data, std::size_t size, Ip4Port_t localPort, + Ip4Port_t remotePort, + const Ip4AddressBytes &remoteAddress) { + auto &pool = *static_cast(arg); + + PacketInfo packet; + packet.destAddr = remoteAddress; + packet.destPort = localPort; + packet.srcPort = remotePort; + + if (size > 0 && data != nullptr) { + packet.payload.assign(data, data + size); + } + + if (!pool.addNewPacket(std::move(packet))) { + pool.logger_.warn("ThreadPool: dropped packet"); + if (pool.isBuiltinPort(remotePort)) { + rtps::Diagnostics::ThreadPool::dropped_incoming_packets_metatraffic++; + } else { + rtps::Diagnostics::ThreadPool::dropped_incoming_packets_usertraffic++; + } + } +} + +void ThreadPool::scheduleReaderWork() { + if (!m_running || !m_readerWorkers) { + return; + } + + (void)m_readerWorkers->try_submit([this]() { doReaderWork(); }); +} + +void ThreadPool::doReaderWork() { + while (m_running) { + PacketInfo packet_user; + auto isUserWorkToDo = m_incomingUserTraffic.moveFirstInto(packet_user); + if (isUserWorkToDo) { + Diagnostics::ThreadPool::processed_incoming_usertraffic++; + m_receiveJumppad(m_callee, const_cast(packet_user)); + } + + PacketInfo packet_meta; + auto isMetaWorkToDo = m_incomingMetaTraffic.moveFirstInto(packet_meta); + if (isMetaWorkToDo) { + Diagnostics::ThreadPool::processed_incoming_metatraffic++; + THREAD_POOL_LOG("ReaderWorker | Processing meta traffic with size {}", static_cast(packet_meta.payload.size())); + m_receiveJumppad(m_callee, const_cast(packet_meta)); + } + + if (isUserWorkToDo || isMetaWorkToDo) { + continue; + } + THREAD_POOL_LOG("ReaderWorker | User = {}, Meta = {}", + static_cast(Diagnostics::ThreadPool::processed_incoming_usertraffic), + static_cast(Diagnostics::ThreadPool::processed_incoming_metatraffic)); + updateDiagnostics(); + return; + } +} + +#undef THREAD_POOL_VERBOSE diff --git a/components/rtps_embedded/src/communication/EsppTransport.cpp b/components/rtps_embedded/src/communication/EsppTransport.cpp new file mode 100644 index 000000000..0f45e677d --- /dev/null +++ b/components/rtps_embedded/src/communication/EsppTransport.cpp @@ -0,0 +1,239 @@ +/* +The MIT License +Copyright (c) 2019 Lehrstuhl Informatik 11 - RWTH Aachen University +Modifications Copyright (c) 2026 ATDev +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: +The above copyright notice and this permission notice shall be included in +all copies or substantial portions of the Software. +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +THE SOFTWARE + +This file is part of embeddedRTPS. + +Author: i11 - Embedded Software, RWTH Aachen University +*/ + +#include "rtps/communication/EsppTransport.h" + +#include "task.hpp" + +#include +#include +#include +#include + +using rtps::EsppTransport; + +namespace { + +bool parseIp4Address(const std::string &address, + rtps::Ip4AddressBytes &out) { + rtps::Ip4AddressBytes parsed{0, 0, 0, 0}; + const char *cursor = address.c_str(); + + for (std::size_t i = 0; i < parsed.size(); ++i) { + char *end = nullptr; + const unsigned long value = std::strtoul(cursor, &end, 10); + if (end == cursor || value > 255) { + return false; + } + + parsed[i] = static_cast(value); + if (i + 1 < parsed.size()) { + if (*end != '.') { + return false; + } + cursor = end + 1; + } else if (*end != '\0') { + return false; + } + } + + out = parsed; + return true; +} + +bool isMulticastAddress(const rtps::Ip4AddressBytes &addr) { + return addr[0] >= 224 && addr[0] <= 239; +} + +} // namespace + +EsppTransport::EsppTransport(RxCallback callback, void *args) + : espp::BaseComponent("RtpsTransport", espp::Logger::Verbosity::WARN), + m_rxCallback(callback), m_callbackArgs(args) {} + +EsppTransport::Channel *EsppTransport::findChannel(Ip4Port_t port) { + for (auto &channel : m_channels) { + if (channel.in_use && channel.port == port) { + return &channel; + } + } + return nullptr; +} + +const EsppTransport::Channel *EsppTransport::findChannel(Ip4Port_t port) const { + for (const auto &channel : m_channels) { + if (channel.in_use && channel.port == port) { + return &channel; + } + } + return nullptr; +} + +std::string EsppTransport::ip4ToString( + const Ip4AddressBytes &addr) { + return std::to_string(addr[0]) + "." + std::to_string(addr[1]) + "." + + std::to_string(addr[2]) + "." + std::to_string(addr[3]); +} + +bool EsppTransport::startReceiver(Channel &channel, Ip4Port_t receivePort) { + if (!channel.socket) { + logger_.error("startReceiver called with null socket on port {}", receivePort); + return false; + } + + espp::Task::BaseConfig task_config; + task_config.name = "rtps_rx_" + std::to_string(receivePort); + // Reuse the reader-worker stack size for the UDP receive task. Both tasks + // perform similar deserialization work, so the value is a reasonable bound. + task_config.stack_size_bytes = Config::THREAD_POOL_READER_STACKSIZE; + task_config.priority = Config::THREAD_POOL_READER_PRIO; + + espp::UdpSocket::ReceiveConfig receive_config; + receive_config.port = receivePort; + receive_config.buffer_size = 1024*8; + receive_config.on_receive_callback = + [this, receivePort](std::vector &data, + const espp::Socket::Info &sender) + -> std::optional> { + onReceive(receivePort, data, sender); + return std::nullopt; + }; + + const bool started = channel.socket->start_receiving(task_config, receive_config); + if (!started) { + logger_.error("Failed to start UDP receiver on port {}", receivePort); + } + return started; +} + +EsppTransport::Channel *EsppTransport::createChannel(Ip4Port_t receivePort) { + for (auto &channel : m_channels) { + if (channel.in_use) { + continue; + } + + espp::UdpSocket::Config socket_config; + socket_config.log_level = espp::Logger::Verbosity::DEBUG; + channel.socket = std::make_unique(socket_config); + if (!channel.socket || !channel.socket->is_valid()) { + logger_.error("Failed to create valid UDP socket for port {}", receivePort); + channel.socket.reset(); + return nullptr; + } + + channel.port = receivePort; + channel.in_use = true; + + if (!startReceiver(channel, receivePort)) { + channel.socket.reset(); + channel.port = 0; + channel.in_use = false; + return nullptr; + } + + for (const auto &group : m_multicastGroups) { + (void)channel.socket->add_multicast_group(group); + } + return &channel; + } + return nullptr; +} + +void EsppTransport::onReceive(Ip4Port_t receivePort, std::vector &data, + const espp::Socket::Info &sender) const { + if (m_rxCallback == nullptr) { + return; + } + + Ip4AddressBytes remoteAddress{0, 0, 0, 0}; + if (!parseIp4Address(sender.address, remoteAddress)) { + logger_.warn("Could not parse sender IPv4 address '{}', using 0.0.0.0", + sender.address); + } + logger_.debug("received {} bytes on port {}", static_cast(data.size()), receivePort); + m_rxCallback(m_callbackArgs, data.data(), data.size(), receivePort, + static_cast(sender.port), remoteAddress); +} + +bool EsppTransport::ensureReceivePort(Ip4Port_t receivePort) { + std::lock_guard lock(m_mutex); + + Channel *existing = findChannel(receivePort); + if (existing != nullptr) { + return true; + } + + Channel *created = createChannel(receivePort); + return created != nullptr; +} + +bool EsppTransport::joinMultiCastGroup( + const Ip4AddressBytes &addr) const { + std::lock_guard lock(m_mutex); + + const std::string group = ip4ToString(addr); + for (const auto &existing : m_multicastGroups) { + if (existing == group) { + return true; + } + } + + bool any_joined = false; + for (auto &channel : m_channels) { + if (!channel.in_use || !channel.socket) { + continue; + } + any_joined = channel.socket->add_multicast_group(group) || any_joined; + } + + m_multicastGroups.push_back(group); + return any_joined || m_multicastGroups.size() == 1; +} + +void EsppTransport::sendPacket(PacketInfo &info) { + std::lock_guard lock(m_mutex); + + Channel *channel = findChannel(info.srcPort); + if (channel == nullptr) { + channel = createChannel(info.srcPort); + } + + if (channel == nullptr || !channel->socket) { + logger_.error("No UDP channel available for source port {}", info.srcPort); + return; + } + + if (info.payload.empty()) { + return; + } + + espp::UdpSocket::SendConfig send_config; + const Ip4AddressBytes destination = info.destAddr; + send_config.ip_address = ip4ToString(destination); + send_config.port = info.destPort; + send_config.is_multicast_endpoint = isMulticastAddress(info.destAddr); + + (void)channel->socket->send(info.payload, send_config); +} diff --git a/components/rtps_embedded/src/discovery/ParticipantProxyData.cpp b/components/rtps_embedded/src/discovery/ParticipantProxyData.cpp new file mode 100644 index 000000000..407898b67 --- /dev/null +++ b/components/rtps_embedded/src/discovery/ParticipantProxyData.cpp @@ -0,0 +1,255 @@ +/* +The MIT License +Copyright (c) 2019 Lehrstuhl Informatik 11 - RWTH Aachen University +Modifications Copyright (c) 2026 ATDev +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: +The above copyright notice and this permission notice shall be included in +all copies or substantial portions of the Software. +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +THE SOFTWARE + +This file is part of embeddedRTPS. + +Author: i11 - Embedded Software, RWTH Aachen University +*/ + +#include "rtps/discovery/ParticipantProxyData.h" +#include "rtps/entities/Participant.h" +#include "rtps/utils/Log.h" + +using rtps::ParticipantProxyData; + +#if SPDP_VERBOSE && RTPS_GLOBAL_VERBOSE +#define PPD_LOG(...) logger_.warn(__VA_ARGS__) +#else +#define PPD_LOG(...) do { } while (0) +#endif + +ParticipantProxyData::ParticipantProxyData(Guid_t guid) + : espp::BaseComponent("RtpsParticipantProxy", espp::Logger::Verbosity::WARN), + m_guid(guid) {} + +void ParticipantProxyData::reset() { + m_guid = Guid_t{GUIDPREFIX_UNKNOWN, ENTITYID_UNKNOWN}; + m_manualLivelinessCount = Count_t{1}; + m_expectsInlineQos = false; + onAliveSignal(); + for (int i = 0; i < Config::SPDP_MAX_NUM_LOCATORS; ++i) { + m_metatrafficUnicastLocatorList[i].setInvalid(); + m_metatrafficMulticastLocatorList[i].setInvalid(); + m_defaultUnicastLocatorList[i].setInvalid(); + m_defaultMulticastLocatorList[i].setInvalid(); + } +} + +bool ParticipantProxyData::readFromUcdrBuffer(ucdrBuffer &buffer, + Participant *participant) { + reset(); + SMElement::ParameterId pid; + uint16_t length; + PPD_LOG("Start deserializing ParticipantProxyData"); + PPD_LOG("Buffer has {} bytes remaining", ucdr_buffer_remaining(&buffer)); + // PPD_LOG("first 20 bytes of data:"); + // for (int i = 0; i < 20 && i < ucdr_buffer_remaining(&buffer); ++i) { + // PPD_LOG("{:02x}", buffer.iterator[i]); + // } + // PPD_LOG("before start, buff length: {}. last data size: {}", + // ucdr_buffer_length(&buffer), buffer.last_data_size); + while (ucdr_buffer_remaining(&buffer) >= 4) { + ucdr_deserialize_uint16_t(&buffer, reinterpret_cast(&pid)); + // PPD_LOG("buff length after get id: {}. last data size: {}", + // ucdr_buffer_length(&buffer), buffer.last_data_size); + + ucdr_deserialize_uint16_t(&buffer, &length); + // PPD_LOG("buff length after get length: {}. last data size: {}", + // ucdr_buffer_length(&buffer), buffer.last_data_size); + PPD_LOG("Deserializing parameter with id {} and length {}", + static_cast(pid), length); + if (ucdr_buffer_remaining(&buffer) < length) { + PPD_LOG("Not enough data left in buffer to read parameter with id {} and length {}", + static_cast(pid), length); + return false; + } + + switch (pid) { + case ParameterId::PID_KEY_HASH: { + // TODO + break; + } + + case ParameterId::PID_PROTOCOL_VERSION: { + ucdr_deserialize_uint8_t(&buffer, &m_protocolVersion.major); + if (m_protocolVersion.major < PROTOCOLVERSION.major) { + PPD_LOG("Unsupported protocol version: {}.{}", m_protocolVersion.major, + m_protocolVersion.minor); + return false; + } else { + ucdr_deserialize_uint8_t(&buffer, &m_protocolVersion.minor); + } + PPD_LOG("Protocol version: {}.{}", m_protocolVersion.major, + m_protocolVersion.minor); + PPD_LOG("buff length: {}. last data size: {}", + ucdr_buffer_length(&buffer), buffer.last_data_size); + break; + } + case ParameterId::PID_VENDORID: { + ucdr_deserialize_array_uint8_t(&buffer, m_vendorId.vendorId.data(), + m_vendorId.vendorId.size()); + PPD_LOG("vendor id struct size: {}", m_vendorId.vendorId.size()); + PPD_LOG("Vendor ID: {} {}", m_vendorId.vendorId[0], + m_vendorId.vendorId[1]); + PPD_LOG("buff length: {}. last data size: {}", + ucdr_buffer_length(&buffer), buffer.last_data_size); + break; + } + + case ParameterId::PID_EXPECTS_INLINE_QOS: { + ucdr_deserialize_bool(&buffer, &m_expectsInlineQos); + break; + } + case ParameterId::PID_PARTICIPANT_GUID: { + ucdr_deserialize_array_uint8_t(&buffer, m_guid.prefix.id.data(), + m_guid.prefix.id.size()); + ucdr_deserialize_array_uint8_t(&buffer, m_guid.entityId.entityKey.data(), + m_guid.entityId.entityKey.size()); + ucdr_deserialize_uint8_t( + &buffer, reinterpret_cast(&m_guid.entityId.entityKind)); + if (participant->findRemoteParticipant(m_guid.prefix)) { + PPD_LOG("stopping deserialization early, participant is known"); + return true; + } + PPD_LOG("Participant GUID: {} {} {} {} {} {} {} {} {} {}", + m_guid.prefix.id[0], m_guid.prefix.id[1], m_guid.prefix.id[2], m_guid.prefix.id[3], + m_guid.prefix.id[4], m_guid.prefix.id[5], m_guid.prefix.id[6], m_guid.prefix.id[7], m_guid.prefix.id[8], m_guid.prefix.id[9]); + break; + } + case ParameterId::PID_METATRAFFIC_MULTICAST_LOCATOR: { + if (!readLocatorIntoList(buffer, m_metatrafficMulticastLocatorList)) { + PPD_LOG("Failed to read metatraffic multicast locator"); + return false; + } + break; + } + case ParameterId::PID_METATRAFFIC_UNICAST_LOCATOR: { + if (!readLocatorIntoList(buffer, m_metatrafficUnicastLocatorList)) { + PPD_LOG("Failed to read metatraffic unicast locator"); + return false; + } + break; + } + case ParameterId::PID_DEFAULT_UNICAST_LOCATOR: { + if (!readLocatorIntoList(buffer, m_defaultUnicastLocatorList)) { + PPD_LOG("Failed to read default unicast locator"); + return false; + } + break; + } + case ParameterId::PID_DEFAULT_MULTICAST_LOCATOR: { + if (!readLocatorIntoList(buffer, m_defaultMulticastLocatorList)) { + PPD_LOG("Failed to read default multicast locator"); + return false; + } + break; + } + case ParameterId::PID_PARTICIPANT_LEASE_DURATION: { + ucdr_deserialize_int32_t(&buffer, &m_leaseDuration.seconds); + ucdr_deserialize_uint32_t(&buffer, &m_leaseDuration.fraction); + break; + } + case ParameterId::PID_BUILTIN_ENDPOINT_SET: { + ucdr_deserialize_uint32_t(&buffer, &m_availableBuiltInEndpoints); + break; + } + case ParameterId::PID_ENTITY_NAME: { + // TODO + buffer.iterator += length; + buffer.last_data_size = 1; + break; + } + case ParameterId::PID_PROPERTY_LIST: { + // TODO + buffer.iterator += length; + buffer.last_data_size = 1; + break; + } + case ParameterId::PID_USER_DATA: { + // TODO + buffer.iterator += length; + buffer.last_data_size = 1; + break; + } + case ParameterId::PID_PAD: { + buffer.iterator += length; + buffer.last_data_size = 1; + break; + } + case ParameterId::PID_SENTINEL: { + return true; + } + default: { + // SPDP_LOG("unknow ID. could be vender defined.\n"); + // should not return false for unknown parameter, just skip it, otherwise we might miss some important information if the remote participant is using some vendor specific parameters that we do not know about. + // TODO: GUO: need read out the data for the length of the parameter, otherwise the buffer will be in wrong state and the following parameters cannot be read correctly. For now just skip the data by moving the iterator forward, but we might want to actually read out the data and store it for future use if needed, especially for some vendor specific parameters that we do not know about. + // buffer.iterator += length; + // buffer.iterator += length; + // buffer.last_data_size = 1; + ucdr_advance_buffer(&buffer, length); + + break; } + } + // Parameter lists are 4-byte aligned + uint32_t alignment = ucdr_buffer_alignment(&buffer, 4); + PPD_LOG("Alignment for next parameter: {}", alignment); + ucdr_advance_buffer(&buffer, alignment); + + // buffer.iterator += alignment; + // buffer.last_data_size = 4; + } + return true; +} + +bool ParticipantProxyData::readLocatorIntoList( + ucdrBuffer &buffer, + std::array &list) { + int valid_locators = 0; + FullLengthLocator full_length_locator; + for (auto &proxy_locator : list) { + if (!proxy_locator.isValid()) { + bool ret = full_length_locator.readFromUcdrBuffer(buffer); + if (ret && full_length_locator.kind == LocatorKind_t::LOCATOR_KIND_UDPv4) { + proxy_locator = LocatorIPv4(full_length_locator); + PPD_LOG("Adding locator: {} {} {} {}", + (int)proxy_locator.address[0], (int)proxy_locator.address[1], + (int)proxy_locator.address[2], (int)proxy_locator.address[3]); + return true; + } else { + PPD_LOG("Ignoring locator: {} {} {} {}", + (int)full_length_locator.address[12], + (int)full_length_locator.address[13], + (int)full_length_locator.address[14], + (int)full_length_locator.address[15]); + return true; + } + } else { + valid_locators++; + if (valid_locators == Config::SPDP_MAX_NUM_LOCATORS) { + buffer.iterator += sizeof(FullLengthLocator); + PPD_LOG("Max number of valid locators exceeded, ignoring this locator as we have at least one valid locator"); + return true; + } + } + } + return false; +} + +#undef PPD_LOG diff --git a/components/rtps_embedded/src/discovery/SEDPAgent.cpp b/components/rtps_embedded/src/discovery/SEDPAgent.cpp new file mode 100644 index 000000000..73c9578a5 --- /dev/null +++ b/components/rtps_embedded/src/discovery/SEDPAgent.cpp @@ -0,0 +1,506 @@ +/* +The MIT License +Copyright (c) 2019 Lehrstuhl Informatik 11 - RWTH Aachen University +Modifications Copyright (c) 2026 ATDev +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: +The above copyright notice and this permission notice shall be included in +all copies or substantial portions of the Software. +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +THE SOFTWARE + +This file is part of embeddedRTPS. + +Author: i11 - Embedded Software, RWTH Aachen University +*/ + +#include "rtps/discovery/SEDPAgent.h" +#include "rtps/discovery/TopicData.h" +#include "rtps/entities/Participant.h" +#include "rtps/entities/Reader.h" +#include "rtps/entities/Writer.h" +#include "rtps/messages/MessageTypes.h" +#include "rtps/utils/Log.h" +#include "ucdr/microcdr.h" +#include + +using rtps::SEDPAgent; + +#if SEDP_VERBOSE && RTPS_GLOBAL_VERBOSE +#define SEDP_LOG(...) logger_.warn(__VA_ARGS__) +#else +#define SEDP_LOG(...) do { } while (0) +#endif + +SEDPAgent::SEDPAgent() + : espp::BaseComponent("RtpsSEDP", espp::Logger::Verbosity::WARN) {} + +void SEDPAgent::init(Participant &part, const BuiltInEndpoints &endpoints) { + m_part = ∂ + m_endpoints = endpoints; + if (m_endpoints.sedpPubReader != nullptr) { + m_endpoints.sedpPubReader->registerCallback(jumppadPublisherReader, this); + } + if (m_endpoints.sedpSubReader != nullptr) { + m_endpoints.sedpSubReader->registerCallback(jumppadSubscriptionReader, + this); + } +} + +void SEDPAgent::registerOnNewPublisherMatchedCallback( + void (*callback)(void *arg), void *args) { + mfp_onNewPublisherCallback = callback; + m_onNewPublisherArgs = args; +} + +void SEDPAgent::registerOnNewSubscriberMatchedCallback( + void (*callback)(void *arg), void *args) { + mfp_onNewSubscriberCallback = callback; + m_onNewSubscriberArgs = args; +} + +void SEDPAgent::jumppadPublisherReader(void *callee, + const ReaderCacheChange &cacheChange) { + auto agent = static_cast(callee); + agent->handlePublisherReaderMessage(cacheChange); +} + +void SEDPAgent::jumppadSubscriptionReader( + void *callee, const ReaderCacheChange &cacheChange) { + auto agent = static_cast(callee); + agent->handleSubscriptionReaderMessage(cacheChange); +} + +void SEDPAgent::handlePublisherReaderMessage(const ReaderCacheChange &change) { + std::lock_guard lock(m_mutex); +#if SEDP_VERBOSE + SEDP_LOG("New publisher"); +#endif + + if (!change.copyInto(m_buffer, sizeof(m_buffer) / sizeof(m_buffer[0]))) { +#if SEDP_VERBOSE + SEDP_LOG("EDPAgent: Buffer too small."); +#endif + return; + } + ucdrBuffer cdrBuffer; + ucdr_init_buffer(&cdrBuffer, m_buffer, sizeof(m_buffer)); + + TopicData topicData; + if (topicData.readFromUcdrBuffer(cdrBuffer)) { + handlePublisherReaderMessage(topicData, change); + } +} + +void SEDPAgent::addUnmatchedRemoteWriter(const TopicData &writerData) { + addUnmatchedRemoteWriter(TopicDataCompressed(writerData)); +} + +void SEDPAgent::addUnmatchedRemoteReader(const TopicData &readerData) { + addUnmatchedRemoteReader(TopicDataCompressed(readerData)); +} + +void SEDPAgent::addUnmatchedRemoteWriter( + const TopicDataCompressed &writerData) { + if (m_unmatchedRemoteWriters.isFull()) { +#if SEDP_VERBOSE + SEDP_LOG("List of unmatched remote writers is full."); +#endif + return; + } + SEDP_LOG("Adding unmatched remote writer {:x} {:x}.", writerData.topicHash, + writerData.typeHash); + m_unmatchedRemoteWriters.add(writerData); +} + +void SEDPAgent::addUnmatchedRemoteReader( + const TopicDataCompressed &readerData) { + if (m_unmatchedRemoteReaders.isFull()) { +#if SEDP_VERBOSE + SEDP_LOG("List of unmatched remote readers is full."); +#endif + return; + } + SEDP_LOG("Adding unmatched remote reader {:x} {:x}.", readerData.topicHash, + readerData.typeHash); + m_unmatchedRemoteReaders.add(readerData); +} + +void SEDPAgent::removeUnmatchedEntity(const Guid_t &guid) { + auto isElementToRemove = [&](const TopicDataCompressed &topicData) { + return topicData.endpointGuid == guid; + }; + + auto thunk = [](void *arg, const TopicDataCompressed &value) { + return (*static_cast(arg))(value); + }; + + m_unmatchedRemoteReaders.remove(thunk, &isElementToRemove); + m_unmatchedRemoteWriters.remove(thunk, &isElementToRemove); +} + +void SEDPAgent::removeUnmatchedEntitiesOfParticipant( + const GuidPrefix_t &guidPrefix) { + std::lock_guard lock(m_mutex); + auto isElementToRemove = [&](const TopicDataCompressed &topicData) { + return topicData.endpointGuid.prefix == guidPrefix; + }; + + auto thunk = [](void *arg, const TopicDataCompressed &value) { + return (*static_cast(arg))(value); + }; + + m_unmatchedRemoteReaders.remove(thunk, &isElementToRemove); + m_unmatchedRemoteWriters.remove(thunk, &isElementToRemove); +} + +uint32_t SEDPAgent::getNumRemoteUnmatchedReaders() { + return m_unmatchedRemoteReaders.getNumElements(); +} + +uint32_t SEDPAgent::getNumRemoteUnmatchedWriters() { + return m_unmatchedRemoteWriters.getNumElements(); +} + +void SEDPAgent::handlePublisherReaderMessage(const TopicData &writerData, + const ReaderCacheChange &change) { + // TODO Is it okay to add Endpoint if the respective participant is unknown + // participant? + if (!m_part->findRemoteParticipant(writerData.endpointGuid.prefix)) { + return; + } + + if (writerData.isDisposedFlagSet() || writerData.isUnregisteredFlagSet()) { + handleRemoteEndpointDeletion(writerData, change); + return; + } + +#if SEDP_VERBOSE + SEDP_LOG("PUB T/D {}/{}", writerData.topicName, writerData.typeName); +#endif + Reader *reader = m_part->getMatchingReader(writerData); + if (reader == nullptr) { +#if SEDP_VERBOSE + SEDP_LOG("SEDPAgent: Couldn't find reader for new Publisher[{}, {}]", + writerData.topicName, writerData.typeName); +#endif + addUnmatchedRemoteWriter(writerData); + return; + } + // TODO check policies +#if SEDP_VERBOSE + if (writerData.reliabilityKind == ReliabilityKind_t::RELIABLE) { + SEDP_LOG("Found a new reliable publisher"); + } else { + SEDP_LOG("Found a new best-effort publisher"); + } +#endif + reader->addNewMatchedWriter( + WriterProxy{writerData.endpointGuid, writerData.unicastLocator, + (writerData.reliabilityKind == ReliabilityKind_t::RELIABLE)}); + if (mfp_onNewPublisherCallback != nullptr) { + mfp_onNewPublisherCallback(m_onNewPublisherArgs); + } +} + +void SEDPAgent::handleSubscriptionReaderMessage( + const ReaderCacheChange &change) { + std::lock_guard lock(m_mutex); +#if SEDP_VERBOSE + SEDP_LOG("New subscriber"); +#endif + + if (!change.copyInto(m_buffer, sizeof(m_buffer) / sizeof(m_buffer[0]))) { +#if SEDP_VERBOSE + SEDP_LOG("SEDPAgent: Buffer too small."); +#endif + return; + } + ucdrBuffer cdrBuffer; + ucdr_init_buffer(&cdrBuffer, m_buffer, sizeof(m_buffer)); + + TopicData topicData; + if (topicData.readFromUcdrBuffer(cdrBuffer)) { + handleSubscriptionReaderMessage(topicData, change); + } +} + +void SEDPAgent::handleRemoteEndpointDeletion(const TopicData &topic, + const ReaderCacheChange &change) { + SEDP_LOG("Endpoint deletion message SN {}.{} GUID {} {} {} {}", + (int)change.sn.high, (int)change.sn.low, + change.writerGuid.prefix.id[0], change.writerGuid.prefix.id[1], + change.writerGuid.prefix.id[2], change.writerGuid.prefix.id[3]); + if (!topic.entityIdFromKeyHashValid) { + return; + } + + Guid_t guid; + guid.prefix = topic.endpointGuid.prefix; + guid.entityId = topic.entityIdFromKeyHash; + + // Remove entity ID from all proxies of local endpoints + m_part->removeProxyFromAllEndpoints(guid); + + // Remove entity ID from unmatched endpoints + removeUnmatchedEntity(guid); +} + +void SEDPAgent::handleSubscriptionReaderMessage( + const TopicData &readerData, const ReaderCacheChange &change) { + if (!m_part->findRemoteParticipant(readerData.endpointGuid.prefix)) { + return; + } + + if (readerData.isDisposedFlagSet() || readerData.isUnregisteredFlagSet()) { + handleRemoteEndpointDeletion(readerData, change); + return; + } + + Writer *writer = m_part->getMatchingWriter(readerData); +#if SEDP_VERBOSE + SEDP_LOG("SUB T/D {}/{}", readerData.topicName, readerData.typeName); +#endif + if (writer == nullptr) { +#if SEDP_VERBOSE + SEDP_LOG("SEDPAgent: Couldn't find writer for new subscriber[{}, {}]", + readerData.topicName, readerData.typeName); +#endif + addUnmatchedRemoteReader(readerData); + return; + } + + // TODO check policies +#if SEDP_VERBOSE + if (readerData.reliabilityKind == ReliabilityKind_t::RELIABLE) { + SEDP_LOG("Found a new reliable subscriber"); + } else { + SEDP_LOG("Found a new best-effort subscriber"); + } +#endif + if (readerData.multicastLocator.kind == + rtps::LocatorKind_t::LOCATOR_KIND_UDPv4) { + writer->addNewMatchedReader(ReaderProxy{ + readerData.endpointGuid, readerData.unicastLocator, + readerData.multicastLocator, + (readerData.reliabilityKind == ReliabilityKind_t::RELIABLE)}); + } else { + writer->addNewMatchedReader(ReaderProxy{ + readerData.endpointGuid, readerData.unicastLocator, + (readerData.reliabilityKind == ReliabilityKind_t::RELIABLE)}); + } + + if (mfp_onNewSubscriberCallback != nullptr) { + mfp_onNewSubscriberCallback(m_onNewSubscriberArgs); + } +} + +void SEDPAgent::tryMatchUnmatchedEndpoints() { + // Try to match remote readers with local writers + for (auto &proxy : m_unmatchedRemoteReaders) { + auto writer = m_part->getMatchingWriter(proxy); + if (writer != nullptr) { + writer->addNewMatchedReader( + ReaderProxy{proxy.endpointGuid, proxy.unicastLocator, + proxy.multicastLocator, proxy.is_reliable}); + removeUnmatchedEntity(proxy.endpointGuid); + } + } + + // Try to match remote writers with local readers + for (auto &proxy : m_unmatchedRemoteWriters) { + auto reader = m_part->getMatchingReader(proxy); + if (reader != nullptr) { + reader->addNewMatchedWriter(WriterProxy{ + proxy.endpointGuid, proxy.unicastLocator, proxy.is_reliable}); + removeUnmatchedEntity(proxy.endpointGuid); + } + } +} + +bool SEDPAgent::addWriter(Writer &writer) { + if (m_endpoints.sedpPubWriter == nullptr) { + return true; + } + EntityKind_t writerKind = + writer.m_attributes.endpointGuid.entityId.entityKind; + if (writerKind == EntityKind_t::BUILD_IN_WRITER_WITH_KEY || + writerKind == EntityKind_t::BUILD_IN_WRITER_WITHOUT_KEY) { + return true; // No need to announce builtin endpoints + } + + std::lock_guard lock(m_mutex); + + // Check unmatched writers for this new reader + tryMatchUnmatchedEndpoints(); + + ucdrBuffer microbuffer; + ucdr_init_buffer(µbuffer, m_buffer, + sizeof(m_buffer) / sizeof(m_buffer[0])); + const uint16_t zero_options = 0; + + ucdr_serialize_array_uint8_t(µbuffer, + rtps::SMElement::SCHEME_PL_CDR_LE.data(), + rtps::SMElement::SCHEME_PL_CDR_LE.size()); + ucdr_serialize_uint16_t(µbuffer, zero_options); + writer.m_attributes.serializeIntoUcdrBuffer(microbuffer); + auto change = m_endpoints.sedpPubWriter->newChange( + ChangeKind_t::ALIVE, m_buffer, ucdr_buffer_length(µbuffer)); + writer.setSEDPSequenceNumber(change->sequenceNumber); + return (change != nullptr); +#if SEDP_VERBOSE + SEDP_LOG("Added new change to sedpPubWriter.\n"); +#endif +} + +template +bool SEDPAgent::disposeEndpointInSEDPHistory(A *local_endpoint, + Writer *sedp_writer) { + return sedp_writer->removeFromHistory( + local_endpoint->getSEDPSequenceNumber()); +} + +template +bool SEDPAgent::announceEndpointDeletion(A *local_endpoint, + Writer *sedp_endpoint) { + ucdrBuffer microbuffer; + ucdr_init_buffer(µbuffer, m_buffer, + sizeof(m_buffer) / sizeof(m_buffer[0])); + + ucdr_serialize_uint16_t(µbuffer, ParameterId::PID_KEY_HASH); + ucdr_serialize_uint16_t(µbuffer, 16); + ucdr_serialize_array_uint8_t( + µbuffer, local_endpoint->m_attributes.endpointGuid.prefix.id.data(), + sizeof(GuidPrefix_t::id)); + ucdr_serialize_array_uint8_t(µbuffer, local_endpoint->m_attributes.endpointGuid.entityId.entityKey.data(), 3); + ucdr_serialize_uint8_t(µbuffer, static_cast(local_endpoint->m_attributes.endpointGuid.entityId.entityKind)); + + ucdr_serialize_uint16_t(µbuffer, ParameterId::PID_STATUS_INFO); + ucdr_serialize_uint16_t(µbuffer, static_cast(4)); + ucdr_serialize_uint8_t(µbuffer, 0); + ucdr_serialize_uint8_t(µbuffer, 0); + ucdr_serialize_uint8_t(µbuffer, 0); + ucdr_serialize_uint8_t(µbuffer, 3); + + // Sentinel to terminate inline qos + ucdr_serialize_uint16_t(µbuffer, ParameterId::PID_SENTINEL); + ucdr_serialize_uint16_t(µbuffer, 0); + + // Sentinel to terminate serialized data + ucdr_serialize_uint16_t(µbuffer, ParameterId::PID_SENTINEL); + ucdr_serialize_uint16_t(µbuffer, 0); + + auto ret = + sedp_endpoint->newChange(ChangeKind_t::ALIVE, m_buffer, + ucdr_buffer_length(µbuffer), true, true); + SEDP_LOG("Announcing endpoint delete, SN = {}.{}", + (int)ret->sequenceNumber.low, (int)ret->sequenceNumber.high); + return (ret != nullptr); +} + +void SEDPAgent::jumppadTakeProxyOfDisposedReader(const Reader *reader, + const WriterProxy &proxy, + void *arg) { + auto agent = static_cast(arg); + TopicDataCompressed topic_data(reader->m_attributes); + topic_data.endpointGuid = proxy.remoteWriterGuid; + topic_data.is_reliable = proxy.is_reliable; + topic_data.multicastLocator.kind = LocatorKind_t::LOCATOR_KIND_INVALID; + topic_data.unicastLocator = proxy.remoteLocator; + agent->addUnmatchedRemoteWriter(topic_data); +} + +void SEDPAgent::jumppadTakeProxyOfDisposedWriter(const Writer *writer, + const ReaderProxy &proxy, + void *arg) { + auto agent = static_cast(arg); + TopicDataCompressed topic_data(writer->m_attributes); + topic_data.endpointGuid = proxy.remoteReaderGuid; + topic_data.is_reliable = proxy.is_reliable; + topic_data.multicastLocator.kind = LocatorKind_t::LOCATOR_KIND_INVALID; + topic_data.unicastLocator = proxy.remoteLocator; + agent->addUnmatchedRemoteReader(topic_data); +} + +bool SEDPAgent::deleteReader(Reader *reader) { + std::lock_guard lock(m_mutex); + // Set cache change kind in SEDP endpoint to DISPOSED + if (!disposeEndpointInSEDPHistory(reader, m_endpoints.sedpSubWriter)) { + return false; + } + + // Create Deletion Message [UD] and add to corret builtin endpoint + if (!announceEndpointDeletion(reader, m_endpoints.sedpSubWriter)) { + return false; + } + + // Move all matched proxies of this endpoint to the list of unmatched + // endpoints + reader->dumpAllProxies(SEDPAgent::jumppadTakeProxyOfDisposedReader, this); + + return true; +} + +bool SEDPAgent::deleteWriter(Writer *writer) { + std::lock_guard lock(m_mutex); + // Set cache change kind in SEDP endpoint to DISPOSED + if (!disposeEndpointInSEDPHistory(writer, m_endpoints.sedpPubWriter)) { + return false; + } + + // Create Deletion Mesasge [UD] and add to corret builtin endpoint + if (!announceEndpointDeletion(writer, m_endpoints.sedpPubWriter)) { + return false; + } + + // Move all matched proxies of this endpoint to the list of unmatched + // endpoints + writer->dumpAllProxies(SEDPAgent::jumppadTakeProxyOfDisposedWriter, this); + + return true; +} + +bool SEDPAgent::addReader(Reader &reader) { + if (m_endpoints.sedpSubWriter == nullptr) { + return true; + } + + EntityKind_t readerKind = + reader.m_attributes.endpointGuid.entityId.entityKind; + if (readerKind == EntityKind_t::BUILD_IN_READER_WITH_KEY || + readerKind == EntityKind_t::BUILD_IN_READER_WITHOUT_KEY) { + return true; // No need to announce builtin endpoints + } + + std::lock_guard lock(m_mutex); + + // Check unmatched writers for this new reader + tryMatchUnmatchedEndpoints(); + + ucdrBuffer microbuffer; + ucdr_init_buffer(µbuffer, m_buffer, + sizeof(m_buffer) / sizeof(m_buffer[0])); + const uint16_t zero_options = 0; + + ucdr_serialize_array_uint8_t(µbuffer, + rtps::SMElement::SCHEME_PL_CDR_LE.data(), + rtps::SMElement::SCHEME_PL_CDR_LE.size()); + ucdr_serialize_uint16_t(µbuffer, zero_options); + reader.m_attributes.serializeIntoUcdrBuffer(microbuffer); + auto change = m_endpoints.sedpSubWriter->newChange( + ChangeKind_t::ALIVE, m_buffer, ucdr_buffer_length(µbuffer)); + reader.setSEDPSequenceNumber(change->sequenceNumber); + return (change != nullptr); +#if SEDP_VERBOSE + SEDP_LOG("Added new change to sedpSubWriter.\n"); +#endif +} diff --git a/components/rtps_embedded/src/discovery/SPDPAgent.cpp b/components/rtps_embedded/src/discovery/SPDPAgent.cpp new file mode 100644 index 000000000..108eb5789 --- /dev/null +++ b/components/rtps_embedded/src/discovery/SPDPAgent.cpp @@ -0,0 +1,387 @@ +/* +The MIT License +Copyright (c) 2019 Lehrstuhl Informatik 11 - RWTH Aachen University +Modifications Copyright (c) 2026 ATDev +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: +The above copyright notice and this permission notice shall be included in +all copies or substantial portions of the Software. +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +THE SOFTWARE + +This file is part of embeddedRTPS. + +Author: i11 - Embedded Software, RWTH Aachen University +*/ + +#include "rtps/discovery/SPDPAgent.h" +#include "rtps/discovery/ParticipantProxyData.h" +#include "rtps/entities/Participant.h" +#include "rtps/entities/Reader.h" +#include "rtps/entities/Writer.h" +#include "rtps/messages/MessageTypes.h" +#include "rtps/utils/Log.h" +#include "rtps/utils/udpUtils.h" + +#include +#include +#include + +using rtps::SPDPAgent; +using rtps::SMElement::BuildInEndpointSet; +using rtps::SMElement::ParameterId; + +SPDPAgent::SPDPAgent() + : espp::BaseComponent("RtpsSPDP", espp::Logger::Verbosity::WARN) {} + +void SPDPAgent::init(Participant &participant, BuiltInEndpoints &endpoints) { + mp_participant = &participant; + m_buildInEndpoints = endpoints; + m_buildInEndpoints.spdpReader->registerCallback(receiveCallback, this); + + ucdr_init_buffer(&m_microbuffer, m_outputBuffer.data(), + m_outputBuffer.size()); + // addInlineQos(); + addParticipantParameters(); + initialized = true; +} + +void SPDPAgent::start() { + if (m_running) { + return; + } + m_running = true; + if (!m_broadcastTask) { + espp::Task::Config config; + config.callback = [this]() { + runBroadcast(); + return true; + }; + config.task_config.name = "SPDPThread"; + config.task_config.stack_size_bytes = Config::SPDP_WRITER_STACKSIZE; + config.task_config.priority = Config::SPDP_WRITER_PRIO; + config.log_level = espp::Logger::Verbosity::WARN; + m_broadcastTask = espp::Task::make_unique(config); + } + (void)m_broadcastTask->start(); +} + +void SPDPAgent::stop() { + m_running = false; + if (m_broadcastTask) { + m_broadcastTask->stop(); + } +} + +void SPDPAgent::runBroadcast() { + const DataSize_t size = ucdr_buffer_length(&m_microbuffer); + const uint8_t *payload = m_microbuffer.init; + m_buildInEndpoints.spdpWriter->newChange(ChangeKind_t::ALIVE, payload, size); + while (m_running) { + std::this_thread::sleep_for( + std::chrono::milliseconds(Config::SPDP_RESEND_PERIOD_MS)); + // StatelessWriter drops already-sent history; enqueue a fresh SPDP sample + // for each announce cycle. + m_buildInEndpoints.spdpWriter->newChange(ChangeKind_t::ALIVE, payload, + size); + if (m_cycleHB == Config::SPDP_CYCLECOUNT_HEARTBEAT) { + m_cycleHB = 0; + mp_participant->checkAndResetHeartbeats(); + } else { + m_cycleHB++; + } + } +} + +void SPDPAgent::receiveCallback(void *callee, + const ReaderCacheChange &cacheChange) { + auto agent = static_cast(callee); + agent->handleSPDPPackage(cacheChange); +} + +void SPDPAgent::handleSPDPPackage(const ReaderCacheChange &cacheChange) { + if (!initialized) { + SPDP_LOG("Callback called without initialization"); + return; + } + + std::lock_guard lock(m_mutex); + if (cacheChange.size > m_inputBuffer.size()) { + SPDP_LOG("Input buffer too small"); + return; + } + + // Something went wrong deserializing remote participant + if (!cacheChange.copyInto(m_inputBuffer.data(), m_inputBuffer.size())) { + return; + } + SPDP_LOG("SPDPPackage size: {}", cacheChange.size); + + ucdrBuffer buffer; + ucdr_init_buffer(&buffer, m_inputBuffer.data(), m_inputBuffer.size()); + + if (cacheChange.kind == ChangeKind_t::ALIVE) { + configureEndianessAndOptions(buffer); + volatile bool success = + m_proxyDataBuffer.readFromUcdrBuffer(buffer, mp_participant); + if (success) { + // TODO In case we store the history we can free the history mutex here + processProxyData(); + } else { + SPDP_LOG("ParticipantProxyData deserialization failed"); + } + } else { + // TODO RemoveParticipant + } +} + +void SPDPAgent::configureEndianessAndOptions(ucdrBuffer &buffer) { + std::array encapsulation{}; + // Endianess doesn't matter for this since those are single bytes + ucdr_deserialize_array_uint8_t(&buffer, encapsulation.data(), + encapsulation.size()); + if (encapsulation == SMElement::SCHEME_PL_CDR_LE) { + buffer.endianness = UCDR_LITTLE_ENDIANNESS; + } else { + buffer.endianness = UCDR_BIG_ENDIANNESS; + } + // Reuse encapsulation buffer to skip options + ucdr_deserialize_array_uint8_t(&buffer, encapsulation.data(), + encapsulation.size()); +} + +void SPDPAgent::processProxyData() { + if (m_proxyDataBuffer.m_guid.prefix.id == mp_participant->m_guidPrefix.id) { + return; // Our own packet + } + + SPDP_LOG("Message from GUID = {} {} {} {}", + m_proxyDataBuffer.m_guid.prefix.id[4], + m_proxyDataBuffer.m_guid.prefix.id[5], + m_proxyDataBuffer.m_guid.prefix.id[6], + m_proxyDataBuffer.m_guid.prefix.id[7]); + const rtps::ParticipantProxyData *remote_part; + remote_part = + mp_participant->findRemoteParticipant(m_proxyDataBuffer.m_guid.prefix); + if (remote_part != nullptr) { + SPDP_LOG("Not adding this participant"); + mp_participant->refreshRemoteParticipantLiveliness( + m_proxyDataBuffer.m_guid.prefix); + return; // Already in our list + } + + if (mp_participant->addNewRemoteParticipant(m_proxyDataBuffer)) { + addProxiesForBuiltInEndpoints(); + const DataSize_t size = ucdr_buffer_length(&m_microbuffer); + m_buildInEndpoints.spdpWriter->newChange(ChangeKind_t::ALIVE, + m_microbuffer.init, size); +#if SPDP_VERBOSE && RTPS_GLOBAL_VERBOSE + SPDP_LOG("Added new participant with guid: "); + printGuidPrefix(m_proxyDataBuffer.m_guid.prefix); + } else { + SPDP_LOG("Failed to add new participant"); + } +#else + } else { + while (1) { + SPDP_LOG("failed to add remote participant"); + } + } +#endif +} + +bool SPDPAgent::addProxiesForBuiltInEndpoints() { + + LocatorIPv4 *locator = nullptr; + + // Check if the remote participants has a locator in our subnet + for (unsigned int i = 0; + i < m_proxyDataBuffer.m_metatrafficUnicastLocatorList.size(); i++) { + LocatorIPv4 *l = &(m_proxyDataBuffer.m_metatrafficUnicastLocatorList[i]); + if (l->isValid() && l->isSameSubnet(mp_participant->m_localIpAddress)) { + locator = l; + break; + } + } + + // Fallback: if subnet check fails or local netif is not fully configured yet, + // still use any valid unicast locator so SEDP matching can proceed. + if (!locator) { + for (unsigned int i = 0; + i < m_proxyDataBuffer.m_metatrafficUnicastLocatorList.size(); i++) { + LocatorIPv4 *l = &(m_proxyDataBuffer.m_metatrafficUnicastLocatorList[i]); + if (l->isValid()) { + locator = l; + break; + } + } + } + + if (!locator) { + return false; + } + + if (m_proxyDataBuffer.hasPublicationReader()) { + const ReaderProxy proxy{{m_proxyDataBuffer.m_guid.prefix, + ENTITYID_SEDP_BUILTIN_PUBLICATIONS_READER}, + *locator, + true}; + m_buildInEndpoints.sedpPubWriter->addNewMatchedReader(proxy); + } + + if (m_proxyDataBuffer.hasSubscriptionReader()) { + const ReaderProxy proxy{{m_proxyDataBuffer.m_guid.prefix, + ENTITYID_SEDP_BUILTIN_SUBSCRIPTIONS_READER}, + *locator, + true}; + m_buildInEndpoints.sedpSubWriter->addNewMatchedReader(proxy); + } + + if (m_proxyDataBuffer.hasPublicationWriter()) { + const WriterProxy proxy{{m_proxyDataBuffer.m_guid.prefix, + ENTITYID_SEDP_BUILTIN_PUBLICATIONS_WRITER}, + *locator, + true}; + m_buildInEndpoints.sedpPubReader->addNewMatchedWriter(proxy); + m_buildInEndpoints.sedpPubReader->sendPreemptiveAckNack(proxy); + } + + if (m_proxyDataBuffer.hasSubscriptionWriter()) { + const WriterProxy proxy{{m_proxyDataBuffer.m_guid.prefix, + ENTITYID_SEDP_BUILTIN_SUBSCRIPTIONS_WRITER}, + *locator, + true}; + m_buildInEndpoints.sedpSubReader->addNewMatchedWriter(proxy); + m_buildInEndpoints.sedpSubReader->sendPreemptiveAckNack(proxy); + } + + return true; +} + +void SPDPAgent::addInlineQos() { + ucdr_serialize_uint16_t(&m_microbuffer, ParameterId::PID_KEY_HASH); + ucdr_serialize_uint16_t(&m_microbuffer, 16); + ucdr_serialize_array_uint8_t(&m_microbuffer, + mp_participant->m_guidPrefix.id.data(), + sizeof(GuidPrefix_t::id)); + ucdr_serialize_array_uint8_t(&m_microbuffer, + ENTITYID_BUILD_IN_PARTICIPANT.entityKey.data(), + sizeof(EntityId_t::entityKey)); + ucdr_serialize_uint8_t( + &m_microbuffer, + static_cast(ENTITYID_BUILD_IN_PARTICIPANT.entityKind)); + + endCurrentList(); +} + +void SPDPAgent::endCurrentList() { + ucdr_serialize_uint16_t(&m_microbuffer, ParameterId::PID_SENTINEL); + ucdr_serialize_uint16_t(&m_microbuffer, 0); +} + +void SPDPAgent::addParticipantParameters() { + const uint16_t zero_options = 0; + const uint16_t protocolVersionSize = + sizeof(PROTOCOLVERSION.major) + sizeof(PROTOCOLVERSION.minor); + const uint16_t vendorIdSize = Config::VENDOR_ID.vendorId.size(); + const uint16_t locatorSize = sizeof(FullLengthLocator); + const uint16_t durationSize = + sizeof(Duration_t::seconds) + sizeof(Duration_t::fraction); + const uint16_t entityKeySize = 3; + const uint16_t entityKindSize = 1; + const uint16_t entityIdSize = entityKeySize + entityKindSize; + const uint16_t guidSize = sizeof(GuidPrefix_t::id) + entityIdSize; + + const FullLengthLocator userUniCastLocator = + getUserUnicastLocator(mp_participant->m_participantId, + mp_participant->m_localIpAddress); + const FullLengthLocator builtInUniCastLocator = + getBuiltInUnicastLocator(mp_participant->m_participantId, + mp_participant->m_localIpAddress); + const FullLengthLocator builtInMultiCastLocator = + getBuiltInMulticastLocator(); + + ucdr_serialize_array_uint8_t(&m_microbuffer, + rtps::SMElement::SCHEME_PL_CDR_LE.data(), + rtps::SMElement::SCHEME_PL_CDR_LE.size()); + ucdr_serialize_uint16_t(&m_microbuffer, zero_options); + + ucdr_serialize_uint16_t(&m_microbuffer, ParameterId::PID_PROTOCOL_VERSION); + ucdr_serialize_uint16_t(&m_microbuffer, protocolVersionSize + 2); + ucdr_serialize_uint8_t(&m_microbuffer, PROTOCOLVERSION.major); + ucdr_serialize_uint8_t(&m_microbuffer, PROTOCOLVERSION.minor); + m_microbuffer.iterator += 2; // padding + m_microbuffer.last_data_size = 4; // to 4 byte + + ucdr_serialize_uint16_t(&m_microbuffer, ParameterId::PID_VENDORID); + ucdr_serialize_uint16_t(&m_microbuffer, vendorIdSize + 2); + ucdr_serialize_array_uint8_t(&m_microbuffer, + Config::VENDOR_ID.vendorId.data(), vendorIdSize); + m_microbuffer.iterator += 2; // padding + m_microbuffer.last_data_size = 4; // to 4 byte + + ucdr_serialize_uint16_t(&m_microbuffer, + ParameterId::PID_DEFAULT_UNICAST_LOCATOR); + ucdr_serialize_uint16_t(&m_microbuffer, locatorSize); + ucdr_serialize_array_uint8_t( + &m_microbuffer, reinterpret_cast(&userUniCastLocator), + locatorSize); + + ucdr_serialize_uint16_t(&m_microbuffer, + ParameterId::PID_METATRAFFIC_UNICAST_LOCATOR); + ucdr_serialize_uint16_t(&m_microbuffer, locatorSize); + ucdr_serialize_array_uint8_t( + &m_microbuffer, reinterpret_cast(&builtInUniCastLocator), + locatorSize); + + ucdr_serialize_uint16_t(&m_microbuffer, + ParameterId::PID_METATRAFFIC_MULTICAST_LOCATOR); + ucdr_serialize_uint16_t(&m_microbuffer, locatorSize); + ucdr_serialize_array_uint8_t( + &m_microbuffer, + reinterpret_cast(&builtInMultiCastLocator), locatorSize); + + ucdr_serialize_uint16_t(&m_microbuffer, + ParameterId::PID_PARTICIPANT_LEASE_DURATION); + ucdr_serialize_uint16_t(&m_microbuffer, durationSize); + ucdr_serialize_int32_t(&m_microbuffer, + Config::SPDP_DEFAULT_REMOTE_LEASE_DURATION.seconds); + ucdr_serialize_uint32_t(&m_microbuffer, + Config::SPDP_DEFAULT_REMOTE_LEASE_DURATION.fraction); + + ucdr_serialize_uint16_t(&m_microbuffer, ParameterId::PID_PARTICIPANT_GUID); + ucdr_serialize_uint16_t(&m_microbuffer, guidSize); + ucdr_serialize_array_uint8_t(&m_microbuffer, + mp_participant->m_guidPrefix.id.data(), + sizeof(GuidPrefix_t::id)); + ucdr_serialize_array_uint8_t(&m_microbuffer, + ENTITYID_BUILD_IN_PARTICIPANT.entityKey.data(), + entityKeySize); + ucdr_serialize_uint8_t( + &m_microbuffer, + static_cast(ENTITYID_BUILD_IN_PARTICIPANT.entityKind)); + + ucdr_serialize_uint16_t(&m_microbuffer, + ParameterId::PID_BUILTIN_ENDPOINT_SET); + ucdr_serialize_uint16_t(&m_microbuffer, sizeof(BuildInEndpointSet)); + ucdr_serialize_uint32_t( + &m_microbuffer, BuildInEndpointSet::DISC_BIE_PARTICIPANT_ANNOUNCER | + BuildInEndpointSet::DISC_BIE_PARTICIPANT_DETECTOR | + BuildInEndpointSet::DISC_BIE_PUBLICATION_ANNOUNCER | + BuildInEndpointSet::DISC_BIE_PUBLICATION_DETECTOR | + BuildInEndpointSet::DISC_BIE_SUBSCRIPTION_ANNOUNCER | + BuildInEndpointSet::DISC_BIE_SUBSCRIPTION_DETECTOR); + + endCurrentList(); +} + +#undef SPDP_VERBOSE diff --git a/components/rtps_embedded/src/discovery/TopicData.cpp b/components/rtps_embedded/src/discovery/TopicData.cpp new file mode 100644 index 000000000..bb65097af --- /dev/null +++ b/components/rtps_embedded/src/discovery/TopicData.cpp @@ -0,0 +1,265 @@ +/* +The MIT License +Copyright (c) 2019 Lehrstuhl Informatik 11 - RWTH Aachen University +Modifications Copyright (c) 2026 ATDev +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: +The above copyright notice and this permission notice shall be included in +all copies or substantial portions of the Software. +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +THE SOFTWARE + +This file is part of embeddedRTPS. + +Author: i11 - Embedded Software, RWTH Aachen University +*/ +#include "rtps/discovery/TopicData.h" +#include "rtps/messages/MessageTypes.h" +#include "logger.hpp" +#include +#include + +using rtps::TopicData; +using rtps::TopicDataCompressed; +using rtps::SMElement::ParameterId; + +namespace { +espp::Logger s_topic_data_logger( + {.tag = "RtpsTopicData", .level = espp::Logger::Verbosity::WARN}); +} + +bool TopicData::isDisposedFlagSet() const { + return statusInfoValid && ((statusInfo & 0b1)); +} + +bool TopicData::isUnregisteredFlagSet() const { + return statusInfoValid && ((statusInfo & (0b1 << 1)) != 0); +} + +bool TopicData::matchesTopicOf(const TopicData &other) { + return strcmp(this->topicName, other.topicName) == 0 && + strcmp(this->typeName, other.typeName) == 0; +} + +bool TopicData::readFromUcdrBuffer(ucdrBuffer &buffer) { + + // Reset valid flags, as the respective parameters are optional + statusInfoValid = false; + entityIdFromKeyHashValid = false; + + while (ucdr_buffer_remaining(&buffer) >= 4) { + if (ucdr_buffer_has_error(&buffer)) { + s_topic_data_logger.error("FAILED TO DESERIALIZE TOPIC DATA"); + return false; + // while (1) { + // printf("FAILED TO DESERIALIZE TOPIC DATA\n"); + // } + } + ParameterId pid; + uint16_t length; + FullLengthLocator uLoc; + ucdr_deserialize_uint16_t(&buffer, reinterpret_cast(&pid)); + ucdr_deserialize_uint16_t(&buffer, &length); + + if (ucdr_buffer_remaining(&buffer) < length) { + return false; + } + + switch (pid) { + case ParameterId::PID_ENDPOINT_GUID: + ucdr_deserialize_array_uint8_t(&buffer, endpointGuid.prefix.id.data(), + endpointGuid.prefix.id.size()); + ucdr_deserialize_array_uint8_t(&buffer, + endpointGuid.entityId.entityKey.data(), + endpointGuid.entityId.entityKey.size()); + ucdr_deserialize_uint8_t(&buffer, reinterpret_cast( + &endpointGuid.entityId.entityKind)); + break; + case ParameterId::PID_RELIABILITY: + ucdr_deserialize_uint32_t(&buffer, + reinterpret_cast(&reliabilityKind)); + buffer.iterator += 8; + // TODO Skip 8 bytes. don't know what they are yet + break; + case ParameterId::PID_SENTINEL: + return true; + case ParameterId::PID_TOPIC_NAME: + uint32_t topicNameLength; + ucdr_deserialize_uint32_t(&buffer, &topicNameLength); + ucdr_deserialize_array_char(&buffer, topicName, topicNameLength); + break; + case ParameterId::PID_TYPE_NAME: + uint32_t typeNameLength; + ucdr_deserialize_uint32_t(&buffer, &typeNameLength); + ucdr_deserialize_array_char(&buffer, typeName, typeNameLength); + break; + case ParameterId::PID_UNICAST_LOCATOR: + uLoc.readFromUcdrBuffer(buffer); + // Accept valid UDPv4 locators even if subnet detection is temporarily + // unavailable (e.g. early startup) to avoid keeping placeholder defaults. + if (uLoc.kind == LocatorKind_t::LOCATOR_KIND_UDPv4) { + unicastLocator = uLoc; + const auto a0 = static_cast(uLoc.address[12]); + const auto a1 = static_cast(uLoc.address[13]); + const auto a2 = static_cast(uLoc.address[14]); + const auto a3 = static_cast(uLoc.address[15]); + const auto port = static_cast(uLoc.port); + s_topic_data_logger.warn( + "Received unicast locator: {}.{}.{}.{}:{}", a0, a1, a2, a3, + port); + } + else { + // print warning and the invalid locator for debugging + s_topic_data_logger.warn( + "Warning: Received invalid unicast locator with kind {}", + static_cast(uLoc.kind)); + } + break; + case ParameterId::PID_MULTICAST_LOCATOR: + multicastLocator.readFromUcdrBuffer(buffer); + break; + case ParameterId::PID_STATUS_INFO: { + if (length == 4) { + buffer.iterator += 3; // skip first 3 bytes of status info as they are + // reserved parameters + ucdr_deserialize_uint8_t(&buffer, &statusInfo); + statusInfoValid = true; + } else { // Ignore Status Info + buffer.iterator += length; + } + } break; + case ParameterId::PID_KEY_HASH: // only use case so far is deleting remote + // endpoints + { + if (length == 16) { + ucdr_deserialize_array_uint8_t(&buffer, endpointGuid.prefix.id.data(), + endpointGuid.prefix.id.size()); + ucdr_deserialize_array_uint8_t( + &buffer, this->entityIdFromKeyHash.entityKey.data(), + this->entityIdFromKeyHash.entityKey.size()); + ucdr_deserialize_uint8_t(&buffer, + reinterpret_cast( + &(this->entityIdFromKeyHash.entityKind))); + entityIdFromKeyHashValid = true; + } else { // Ignore value + buffer.iterator += length; + } + } break; + default: + ucdr_advance_buffer(&buffer, length); + // buffer.iterator += length; + // buffer.last_data_size = 1; + } + + uint32_t alignment = ucdr_buffer_alignment(&buffer, 4); + ucdr_advance_buffer(&buffer, alignment); + // buffer.iterator += alignment; + // buffer.last_data_size = 4; // 4 Byte alignment per element + } + return ucdr_buffer_remaining(&buffer) == 0; +} + +bool TopicData::serializeIntoUcdrBuffer(ucdrBuffer &buffer) const { + // TODO Check if buffer length is sufficient + const uint16_t guidSize = sizeof(GuidPrefix_t::id) + 4; + +#if SUPPRESS_UNICAST + if (multicastLocator.kind != LocatorKind_t::LOCATOR_KIND_UDPv4) { +#endif + ucdr_serialize_uint16_t(&buffer, ParameterId::PID_UNICAST_LOCATOR); + ucdr_serialize_uint16_t(&buffer, sizeof(FullLengthLocator)); + ucdr_serialize_array_uint8_t( + &buffer, reinterpret_cast(&unicastLocator), + sizeof(FullLengthLocator)); +#if SUPPRESS_UNICAST + } +#endif + + if (multicastLocator.kind == LocatorKind_t::LOCATOR_KIND_UDPv4) { + ucdr_serialize_uint16_t(&buffer, ParameterId::PID_MULTICAST_LOCATOR); + ucdr_serialize_uint16_t(&buffer, sizeof(FullLengthLocator)); + ucdr_serialize_array_uint8_t( + &buffer, reinterpret_cast(&multicastLocator), + sizeof(FullLengthLocator)); + } + + // It's a 32 bit instead of 16 because it seems like the field is padded. + const auto lenTopicName = + static_cast(strlen(topicName) + 1); // + \0 + uint16_t topicAlignment = 0; + if (lenTopicName % 4 != 0) { + topicAlignment = static_cast(4 - (lenTopicName % 4)); + } + const auto totalLengthTopicNameField = static_cast( + sizeof(lenTopicName) + lenTopicName + topicAlignment); + ucdr_serialize_uint16_t(&buffer, ParameterId::PID_TOPIC_NAME); + ucdr_serialize_uint16_t(&buffer, totalLengthTopicNameField); + ucdr_serialize_uint32_t(&buffer, lenTopicName); + ucdr_serialize_array_char(&buffer, topicName, lenTopicName); + ucdr_align_to(&buffer, 4); + + // It's a 32 bit instead of 16 because it seems like the field is padded. + const auto lenTypeName = static_cast(strlen(typeName) + 1); // + \0 + uint16_t typeAlignment = 0; + if (lenTypeName % 4 != 0) { + typeAlignment = static_cast(4 - (lenTypeName % 4)); + } + const auto totalLengthTypeNameField = + static_cast(sizeof(lenTypeName) + lenTypeName + typeAlignment); + + ucdr_serialize_uint16_t(&buffer, ParameterId::PID_TYPE_NAME); + ucdr_serialize_uint16_t(&buffer, totalLengthTypeNameField); + ucdr_serialize_uint32_t(&buffer, lenTypeName); + ucdr_serialize_array_char(&buffer, typeName, lenTypeName); + ucdr_align_to(&buffer, 4); + + ucdr_serialize_uint16_t(&buffer, ParameterId::PID_KEY_HASH); + ucdr_serialize_uint16_t(&buffer, guidSize); + ucdr_serialize_array_uint8_t(&buffer, endpointGuid.prefix.id.data(), + endpointGuid.prefix.id.size()); + ucdr_serialize_array_uint8_t(&buffer, endpointGuid.entityId.entityKey.data(), + endpointGuid.entityId.entityKey.size()); + ucdr_serialize_uint8_t( + &buffer, static_cast(endpointGuid.entityId.entityKind)); + + ucdr_serialize_uint16_t(&buffer, ParameterId::PID_ENDPOINT_GUID); + ucdr_serialize_uint16_t(&buffer, guidSize); + ucdr_serialize_array_uint8_t(&buffer, endpointGuid.prefix.id.data(), + endpointGuid.prefix.id.size()); + ucdr_serialize_array_uint8_t(&buffer, endpointGuid.entityId.entityKey.data(), + endpointGuid.entityId.entityKey.size()); + ucdr_serialize_uint8_t( + &buffer, static_cast(endpointGuid.entityId.entityKind)); + + const uint8_t unidentifiedOffset = 8; + ucdr_serialize_uint16_t(&buffer, ParameterId::PID_RELIABILITY); + ucdr_serialize_uint16_t(&buffer, + sizeof(ReliabilityKind_t) + unidentifiedOffset); + ucdr_serialize_uint32_t(&buffer, static_cast(reliabilityKind)); + ucdr_serialize_uint32_t(&buffer, 0); // unidentified additional value + ucdr_serialize_uint32_t(&buffer, 0); // unidentified additional value + + ucdr_serialize_uint16_t(&buffer, ParameterId::PID_DURABILITY); + ucdr_serialize_uint16_t(&buffer, sizeof(DurabilityKind_t)); + ucdr_serialize_uint32_t(&buffer, static_cast(durabilityKind)); + + ucdr_serialize_uint16_t(&buffer, ParameterId::PID_SENTINEL); + ucdr_serialize_uint16_t(&buffer, 0); + + return true; +} + +bool TopicDataCompressed::matchesTopicOf(const TopicData &other) const { + return (hashCharArray(other.topicName, sizeof(other.topicName)) == + topicHash && + hashCharArray(other.typeName, sizeof(other.typeName)) == typeHash); +} diff --git a/components/rtps_embedded/src/entities/Domain.cpp b/components/rtps_embedded/src/entities/Domain.cpp new file mode 100644 index 000000000..72f5b6044 --- /dev/null +++ b/components/rtps_embedded/src/entities/Domain.cpp @@ -0,0 +1,568 @@ +/* +The MIT License +Copyright (c) 2019 Lehrstuhl Informatik 11 - RWTH Aachen University +Modifications Copyright (c) 2026 ATDev +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: +The above copyright notice and this permission notice shall be included in +all copies or substantial portions of the Software. +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +THE SOFTWARE + +This file is part of embeddedRTPS. + +Author: i11 - Embedded Software, RWTH Aachen University +*/ + +#include "rtps/entities/Domain.h" +#include "rtps/utils/Log.h" +#include "rtps/utils/udpUtils.h" +#include +#include + +#if defined(ESP_PLATFORM) +#include "esp_mac.h" +#endif + +#if DOMAIN_VERBOSE && RTPS_GLOBAL_VERBOSE +#define DOMAIN_LOG(...) logger_.warn(__VA_ARGS__) +#else +#define DOMAIN_LOG(...) do { } while (0) +#endif + +using rtps::Domain; + +Domain::Domain(const rtps::Ip4AddressBytes &localIpAddress) + : espp::BaseComponent("RtpsDomain", espp::Logger::Verbosity::WARN), + m_threadPool(receiveJumppad, this), + m_defaultTransport(ThreadPool::onDatagram, &m_threadPool), + m_transport(&m_defaultTransport), + m_localIpAddress(localIpAddress) { + initializeTransport(); +} + +Domain::Domain(rtps::EsppTransport &transport, + const rtps::Ip4AddressBytes &localIpAddress) + : espp::BaseComponent("RtpsDomain", espp::Logger::Verbosity::WARN), + m_threadPool(receiveJumppad, this), + m_defaultTransport(ThreadPool::onDatagram, &m_threadPool), + m_transport(&transport), + m_localIpAddress(localIpAddress) { + initializeTransport(); +} + +void Domain::initializeTransport() { + assert(m_transport != nullptr); + m_transport->ensureReceivePort(getUserMulticastPort()); + m_transport->ensureReceivePort(getBuiltInMulticastPort()); + m_transport->joinMultiCastGroup({239, 255, 0, 1}); +} + +Domain::~Domain() { stop(); } + +bool Domain::completeInit() { + m_initComplete = m_threadPool.startThreads(); + + if (!m_initComplete) { + DOMAIN_LOG("Failed starting threads"); + } + + for (auto i = 0; i < m_nextParticipantId; i++) { + m_participants[i].getSPDPAgent().start(); + } + return m_initComplete; +} + +void Domain::stop() { m_threadPool.stopThreads(); } + +void Domain::receiveJumppad(void *callee, const PacketInfo &packet) { + auto domain = static_cast(callee); + domain->receiveCallback(packet); +} + +void Domain::receiveCallback(const PacketInfo &packet) { + if (packet.payload.empty()) { + DOMAIN_LOG("Dropping packet without payload"); + return; + } + + const uint8_t *payload = packet.payload.data(); + DataSize_t payload_size = static_cast(packet.payload.size()); + + if (isMetaMultiCastPort(packet.destPort)) { + // Pass to all + DOMAIN_LOG("Domain: Multicast to port {}", packet.destPort); + for (auto i = 0; i < m_nextParticipantId - PARTICIPANT_START_ID; ++i) { + m_participants[i].newMessage(payload, payload_size); + } + // First Check if UserTraffic Multicast + } else if (isUserMultiCastPort(packet.destPort)) { + // Pass to Participant with assigned Multicast Adress (Port ist everytime + // the same) + DOMAIN_LOG("Domain: Got user multicast message on port {}", + packet.destPort); + for (auto i = 0; i < m_nextParticipantId - PARTICIPANT_START_ID; ++i) { + if (m_participants[i].hasReaderWithMulticastLocator(packet.destAddr)) { + DOMAIN_LOG("Domain: Forward Multicast only to Participant: {}", i); + m_participants[i].newMessage(payload, payload_size); + } + } + } else { + // Pass to addressed one only (Unicast, by Port) + ParticipantId_t id = getParticipantIdFromUnicastPort( + packet.destPort, isUserPort(packet.destPort)); + if (id != PARTICIPANT_ID_INVALID) { + DOMAIN_LOG("Domain: Got unicast message on port {}", packet.destPort); + if (id < m_nextParticipantId && + id >= PARTICIPANT_START_ID) { // added extra check to avoid segfault + // (id below START_ID) + m_participants[id - PARTICIPANT_START_ID].newMessage( + payload, payload_size); + } else { + DOMAIN_LOG("Domain: Participant id too high or unplausible."); + } + } else { + DOMAIN_LOG("Domain: Got message to port {}: no matching participant", + packet.destPort); + } + } +} + +rtps::Participant *Domain::createParticipant() { + + DOMAIN_LOG("Domain: Creating new participant."); + + auto nextSlot = + static_cast(m_nextParticipantId - PARTICIPANT_START_ID); + if (m_initComplete || m_participants.size() <= nextSlot) { + return nullptr; + } + + auto &entry = m_participants[nextSlot]; + entry.reuse(generateGuidPrefix(m_nextParticipantId), m_nextParticipantId, + m_localIpAddress); + registerPort(entry); + createBuiltinWritersAndReaders(entry); + ++m_nextParticipantId; + return &entry; +} + +void Domain::createBuiltinWritersAndReaders(Participant &part) { + // SPDP + StatelessWriter *spdpWriter = + getNextUnusedEndpoint( + m_statelessWriters); + StatelessReader *spdpReader = + getNextUnusedEndpoint( + m_statelessReaders); + + TopicData spdpWriterAttributes; + spdpWriterAttributes.topicName[0] = '\0'; + spdpWriterAttributes.typeName[0] = '\0'; + spdpWriterAttributes.reliabilityKind = ReliabilityKind_t::BEST_EFFORT; + spdpWriterAttributes.durabilityKind = DurabilityKind_t::TRANSIENT_LOCAL; + spdpWriterAttributes.endpointGuid.prefix = part.m_guidPrefix; + spdpWriterAttributes.endpointGuid.entityId = + ENTITYID_SPDP_BUILTIN_PARTICIPANT_WRITER; + spdpWriterAttributes.unicastLocator = getBuiltInMulticastLocator(); + + spdpWriter->init(spdpWriterAttributes, TopicKind_t::WITH_KEY, &m_threadPool, + *m_transport); + spdpWriter->addNewMatchedReader( + ReaderProxy{{part.m_guidPrefix, ENTITYID_SPDP_BUILTIN_PARTICIPANT_READER}, + getBuiltInMulticastLocator(), + false}); + + TopicData spdpReaderAttributes; + spdpReaderAttributes.endpointGuid = { + part.m_guidPrefix, ENTITYID_SPDP_BUILTIN_PARTICIPANT_READER}; + spdpReader->init(spdpReaderAttributes); + + // SEDP + + // Prepare attributes + TopicData sedpAttributes; + sedpAttributes.topicName[0] = '\0'; + sedpAttributes.typeName[0] = '\0'; + sedpAttributes.reliabilityKind = ReliabilityKind_t::RELIABLE; + sedpAttributes.durabilityKind = DurabilityKind_t::TRANSIENT_LOCAL; + sedpAttributes.endpointGuid.prefix = part.m_guidPrefix; + sedpAttributes.unicastLocator = + getBuiltInUnicastLocator(part.m_participantId, m_localIpAddress); + + // READER + StatefulReader *sedpPubReader = + getNextUnusedEndpoint( + m_statefulReaders); + sedpAttributes.endpointGuid.entityId = + ENTITYID_SEDP_BUILTIN_PUBLICATIONS_READER; + sedpPubReader->init(sedpAttributes, *m_transport); + + StatefulReader *sedpSubReader = + getNextUnusedEndpoint( + m_statefulReaders); + sedpAttributes.endpointGuid.entityId = + ENTITYID_SEDP_BUILTIN_SUBSCRIPTIONS_READER; + sedpSubReader->init(sedpAttributes, *m_transport); + + // WRITER + StatefulWriter *sedpPubWriter = + getNextUnusedEndpoint( + m_statefulWriters); + sedpAttributes.endpointGuid.entityId = + ENTITYID_SEDP_BUILTIN_PUBLICATIONS_WRITER; + sedpPubWriter->init(sedpAttributes, TopicKind_t::NO_KEY, &m_threadPool, + *m_transport); + + StatefulWriter *sedpSubWriter = + getNextUnusedEndpoint( + m_statefulWriters); + sedpAttributes.endpointGuid.entityId = + ENTITYID_SEDP_BUILTIN_SUBSCRIPTIONS_WRITER; + sedpSubWriter->init(sedpAttributes, TopicKind_t::NO_KEY, &m_threadPool, + *m_transport); + + // COLLECT + BuiltInEndpoints endpoints{}; + endpoints.spdpWriter = spdpWriter; + endpoints.spdpReader = spdpReader; + endpoints.sedpPubReader = sedpPubReader; + endpoints.sedpSubReader = sedpSubReader; + endpoints.sedpPubWriter = sedpPubWriter; + endpoints.sedpSubWriter = sedpSubWriter; + + part.addBuiltInEndpoints(endpoints); +} + +void Domain::registerPort(const Participant &part) { + m_transport->ensureReceivePort(getUserUnicastPort(part.m_participantId)); + m_transport->ensureReceivePort(getBuiltInUnicastPort(part.m_participantId)); + m_threadPool.addBuiltinPort(getBuiltInUnicastPort(part.m_participantId)); +} + +void Domain::registerMulticastPort(FullLengthLocator mcastLocator) { + if (mcastLocator.kind == LocatorKind_t::LOCATOR_KIND_UDPv4) { + m_transport->ensureReceivePort(mcastLocator.getLocatorPort()); + } +} + +rtps::Reader *Domain::readerExists(Participant &part, const char *topicName, + const char *typeName, bool reliable) { + std::lock_guard lock(m_mutex); + if (reliable) { + for (unsigned int i = 0; i < m_statefulReaders.size(); i++) { + if (m_statefulReaders[i].isInitialized()) { + if (strncmp(m_statefulReaders[i].m_attributes.topicName, topicName, + Config::MAX_TYPENAME_LENGTH) != 0) { + continue; + } + + if (strncmp(m_statefulReaders[i].m_attributes.typeName, typeName, + Config::MAX_TYPENAME_LENGTH) != 0) { + continue; + } + + DOMAIN_LOG("StatefulReader exists already [{}, {}]", topicName, + typeName); + + return &m_statefulReaders[i]; + } + } + } else { + for (unsigned int i = 0; i < m_statelessReaders.size(); i++) { + if (m_statelessReaders[i].isInitialized()) { + if (strncmp(m_statelessReaders[i].m_attributes.topicName, topicName, + Config::MAX_TYPENAME_LENGTH) != 0) { + continue; + } + + if (strncmp(m_statelessReaders[i].m_attributes.typeName, typeName, + Config::MAX_TYPENAME_LENGTH) != 0) { + continue; + } + + DOMAIN_LOG("StatelessReader exists [{}, {}]", topicName, typeName); + + return &m_statelessReaders[i]; + } + } + } + + return nullptr; +} + +rtps::Writer *Domain::writerExists(Participant &part, const char *topicName, + const char *typeName, bool reliable) { + std::lock_guard lock(m_mutex); + if (reliable) { + for (unsigned int i = 0; i < m_statefulWriters.size(); i++) { + if (m_statefulWriters[i].isInitialized()) { + if (strncmp(m_statefulWriters[i].m_attributes.topicName, topicName, + Config::MAX_TYPENAME_LENGTH) != 0) { + continue; + } + + if (strncmp(m_statefulWriters[i].m_attributes.typeName, typeName, + Config::MAX_TYPENAME_LENGTH) != 0) { + continue; + } + + DOMAIN_LOG("StatefulWriter exists [{}, {}]", topicName, typeName); + + return &m_statefulWriters[i]; + } + } + } else { + for (unsigned int i = 0; i < m_statelessWriters.size(); i++) { + if (m_statelessWriters[i].isInitialized()) { + if (strncmp(m_statelessWriters[i].m_attributes.topicName, topicName, + Config::MAX_TYPENAME_LENGTH) != 0) { + continue; + } + + if (strncmp(m_statelessWriters[i].m_attributes.typeName, typeName, + Config::MAX_TYPENAME_LENGTH) != 0) { + continue; + } + + DOMAIN_LOG("StatelessWriter exists [{}, {}]", topicName, typeName); + + return &m_statelessWriters[i]; + } + } + } + + return nullptr; +} + +rtps::Writer *Domain::createWriter(Participant &part, const char *topicName, + const char *typeName, bool reliable, + bool enforceUnicast) { + std::lock_guard lock(m_mutex); + StatelessWriter *statelessWriter = + getNextUnusedEndpoint( + m_statelessWriters); + StatefulWriter *statefulWriter = + getNextUnusedEndpoint( + m_statefulWriters); + + // Check if there is enough capacity for more writers + if ((reliable && statefulWriter == nullptr) || + (!reliable && statelessWriter == nullptr) || part.isWritersFull()) { + + DOMAIN_LOG("No Writer created. Max Number of Writers reached."); + + return nullptr; + } + + // TODO Distinguish WithKey and NoKey (Also changes EntityKind) + TopicData attributes; + + if (strlen(topicName) > Config::MAX_TOPICNAME_LENGTH || + strlen(typeName) > Config::MAX_TYPENAME_LENGTH) { + return nullptr; + } + strcpy(attributes.topicName, topicName); + strcpy(attributes.typeName, typeName); + attributes.endpointGuid.prefix = part.m_guidPrefix; + attributes.endpointGuid.entityId = { + part.getNextUserEntityKey(), + EntityKind_t::USER_DEFINED_WRITER_WITHOUT_KEY}; + attributes.unicastLocator = + getUserUnicastLocator(part.m_participantId, m_localIpAddress); + attributes.durabilityKind = DurabilityKind_t::TRANSIENT_LOCAL; + + DOMAIN_LOG("Creating writer[{}, {}]", topicName, typeName); + + if (reliable) { + attributes.reliabilityKind = ReliabilityKind_t::RELIABLE; + + if (!statefulWriter->init(attributes, TopicKind_t::NO_KEY, &m_threadPool, + *m_transport, enforceUnicast)) { + DOMAIN_LOG("StatefulWriter init failed."); + return nullptr; + } + + if (!part.addWriter(statefulWriter)) { + return nullptr; + } + return statefulWriter; + } else { + attributes.reliabilityKind = ReliabilityKind_t::BEST_EFFORT; + + if (!statelessWriter->init(attributes, TopicKind_t::NO_KEY, &m_threadPool, + *m_transport, enforceUnicast)) { + DOMAIN_LOG("StatelessWriter init failed."); + return nullptr; + } + + if (!part.addWriter(statelessWriter)) { + return nullptr; + } + return statelessWriter; + } +} + +rtps::Reader *Domain::createReader(Participant &part, const char *topicName, + const char *typeName, bool reliable, + rtps::Ip4AddressBytes mcastaddress) { + std::lock_guard lock(m_mutex); + StatelessReader *statelessReader = + getNextUnusedEndpoint( + m_statelessReaders); + StatefulReader *statefulReader = + getNextUnusedEndpoint( + m_statefulReaders); + + if ((reliable && statefulReader == nullptr) || + (!reliable && statelessReader == nullptr) || part.isReadersFull()) { + + DOMAIN_LOG("No Reader created. Max Number of Readers reached."); + + return nullptr; + } + + // TODO Distinguish WithKey and NoKey (Also changes EntityKind) + TopicData attributes; + + if (strlen(topicName) > Config::MAX_TOPICNAME_LENGTH || + strlen(typeName) > Config::MAX_TYPENAME_LENGTH) { + return nullptr; + } + strcpy(attributes.topicName, topicName); + strcpy(attributes.typeName, typeName); + attributes.endpointGuid.prefix = part.m_guidPrefix; + attributes.endpointGuid.entityId = { + part.getNextUserEntityKey(), + EntityKind_t::USER_DEFINED_READER_WITHOUT_KEY}; + attributes.unicastLocator = + getUserUnicastLocator(part.m_participantId, m_localIpAddress); + if (!isZeroAddress(mcastaddress)) { + if (isMulticastAddress(mcastaddress)) { + attributes.multicastLocator = rtps::FullLengthLocator::createUDPv4Locator( + mcastaddress[0], mcastaddress[1], mcastaddress[2], mcastaddress[3], + getUserMulticastPort()); + m_transport->joinMultiCastGroup( + {attributes.multicastLocator.address[12], + attributes.multicastLocator.address[13], + attributes.multicastLocator.address[14], + attributes.multicastLocator.address[15]}); + registerMulticastPort(attributes.multicastLocator); + + DOMAIN_LOG("Multicast enabled!"); + + } else { + + DOMAIN_LOG("This is not a Multicastaddress!"); + } + } + attributes.durabilityKind = DurabilityKind_t::VOLATILE; + + DOMAIN_LOG("Creating reader[{}, {}]", topicName, typeName); + + if (reliable) { + + attributes.reliabilityKind = ReliabilityKind_t::RELIABLE; + + statefulReader->init(attributes, *m_transport); + + if (!part.addReader(statefulReader)) { + DOMAIN_LOG("Failed to add reader to participant."); + + return nullptr; + } + return statefulReader; + } else { + + attributes.reliabilityKind = ReliabilityKind_t::BEST_EFFORT; + + statelessReader->init(attributes); + + if (!part.addReader(statelessReader)) { + return nullptr; + } + return statelessReader; + } +} + +bool rtps::Domain::deleteReader(Participant &part, Reader *reader) { + std::lock_guard lock(m_mutex); + if(reader == nullptr || !reader->isInitialized()){ + return false; + } + if (!part.deleteReader(reader)) { + return false; + } + + reader->reset(); + return true; +} + +bool rtps::Domain::deleteWriter(Participant &part, Writer *writer) { + std::lock_guard lock(m_mutex); + if(writer == nullptr || !writer->isInitialized()){ + return false; + } + if (!part.deleteWriter(writer)) { + return false; + } + + writer->reset(); + return true; +} + +void rtps::Domain::printInfo() { + for (unsigned int i = 0; i < m_participants.size(); i++) { + DOMAIN_LOG("Participant {}", i); + m_participants[i].printInfo(); + } +} + +rtps::GuidPrefix_t Domain::generateGuidPrefix(ParticipantId_t id) const { + GuidPrefix_t prefix; +#if defined(ESP_PLATFORM) + uint8_t mac[6] = {0}; + esp_err_t mac_err = esp_read_mac(mac, ESP_MAC_ETH); + if (mac_err != ESP_OK) { + mac_err = esp_read_mac(mac, ESP_MAC_WIFI_STA); + } + if (mac_err == ESP_OK) { + // Make participant GUID unique per board while keeping a stable layout. + prefix.id[0] = mac[0]; + prefix.id[1] = mac[1]; + prefix.id[2] = mac[2]; + prefix.id[3] = mac[3]; + prefix.id[4] = mac[4]; + prefix.id[5] = mac[5]; + prefix.id[6] = static_cast(id); + prefix.id[7] = Config::VENDOR_ID.vendorId[0]; + prefix.id[8] = Config::VENDOR_ID.vendorId[1]; + prefix.id[9] = Config::DOMAIN_ID; + prefix.id[10] = 0xA5; + prefix.id[11] = 0x5A; + return prefix; + } +#endif + + if (Config::BASE_GUID_PREFIX == GUID_RANDOM) { + for (unsigned int i = 0; i < rtps::Config::BASE_GUID_PREFIX.id.size(); + i++) { + prefix.id[i] = rand(); + } + } else { + for (unsigned int i = 0; i < rtps::Config::BASE_GUID_PREFIX.id.size(); + i++) { + prefix.id[i] = Config::BASE_GUID_PREFIX.id[i]; + } + } + return prefix; +} diff --git a/components/rtps_embedded/src/entities/Participant.cpp b/components/rtps_embedded/src/entities/Participant.cpp new file mode 100644 index 000000000..7e62f973c --- /dev/null +++ b/components/rtps_embedded/src/entities/Participant.cpp @@ -0,0 +1,546 @@ +/* +The MIT License +Copyright (c) 2019 Lehrstuhl Informatik 11 - RWTH Aachen University +Modifications Copyright (c) 2026 ATDev +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: +The above copyright notice and this permission notice shall be included in +all copies or substantial portions of the Software. +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +THE SOFTWARE + +This file is part of embeddedRTPS. + +Author: i11 - Embedded Software, RWTH Aachen University +*/ + +#include "rtps/entities/Participant.h" +#include "rtps/entities/Reader.h" +#include "rtps/entities/Writer.h" +#include "rtps/messages/MessageReceiver.h" +#include "rtps/utils/Log.h" +#include + +#if PARTICIPANT_VERBOSE && RTPS_GLOBAL_VERBOSE +#define PARTICIPANT_LOG(...) logger_.warn(__VA_ARGS__) +#else +#define PARTICIPANT_LOG(...) do { } while (0) +#endif + +using rtps::Participant; + +Participant::Participant() + : espp::BaseComponent("RtpsParticipant", espp::Logger::Verbosity::WARN), + m_guidPrefix(GUIDPREFIX_UNKNOWN), m_participantId(PARTICIPANT_ID_INVALID), + m_receiver(this) {} +Participant::Participant(const GuidPrefix_t &guidPrefix, + ParticipantId_t participantId) + : espp::BaseComponent("RtpsParticipant", espp::Logger::Verbosity::WARN), + m_guidPrefix(guidPrefix), m_participantId(participantId), + m_receiver(this) {} + +Participant::~Participant() { m_spdpAgent.stop(); } + +void Participant::reuse(const GuidPrefix_t &guidPrefix, + ParticipantId_t participantId) { + m_guidPrefix = guidPrefix; + m_participantId = participantId; + m_localIpAddress = {Config::IP_ADDRESS[0], Config::IP_ADDRESS[1], + Config::IP_ADDRESS[2], Config::IP_ADDRESS[3]}; +} + +void Participant::reuse( + const GuidPrefix_t &guidPrefix, ParticipantId_t participantId, + const Ip4AddressBytes &localIpAddress) { + m_guidPrefix = guidPrefix; + m_participantId = participantId; + m_localIpAddress = localIpAddress; +} + +bool Participant::isValid() { + return m_participantId != PARTICIPANT_ID_INVALID; +} + +std::array Participant::getNextUserEntityKey() { + const auto result = m_nextUserEntityId; + + ++m_nextUserEntityId[2]; + if (m_nextUserEntityId[2] == 0) { + ++m_nextUserEntityId[1]; + if (m_nextUserEntityId[1] == 0) { + ++m_nextUserEntityId[0]; + } + } + return result; +} + +bool Participant::registerOnNewPublisherMatchedCallback( + void (*callback)(void *arg), void *args) { + if (!m_hasBuilInEndpoints) { + return false; + } + + m_sedpAgent.registerOnNewPublisherMatchedCallback(callback, args); + return true; +} + +bool Participant::registerOnNewSubscriberMatchedCallback( + void (*callback)(void *arg), void *args) { + if (!m_hasBuilInEndpoints) { + return false; + } + + m_sedpAgent.registerOnNewSubscriberMatchedCallback(callback, args); + return true; +} + +rtps::Writer *Participant::addWriter(Writer *pWriter) { + std::lock_guard lock(m_mutex); + for (unsigned int i = 0; i < m_writers.size(); i++) { + if (m_writers[i] == nullptr) { + m_writers[i] = pWriter; + if (m_hasBuilInEndpoints) { + m_sedpAgent.addWriter(*pWriter); + } + return pWriter; + } + } + return nullptr; +} + +bool Participant::isWritersFull() { + std::lock_guard lock(m_mutex); + for (unsigned int i = 0; i < m_writers.size(); i++) { + if (m_writers[i] == nullptr) { + return false; + } + } + + return true; +} + +rtps::Reader *Participant::addReader(Reader *pReader) { + std::lock_guard lock(m_mutex); + for (unsigned int i = 0; i < m_readers.size(); i++) { + if (m_readers[i] == nullptr) { + m_readers[i] = pReader; + if (m_hasBuilInEndpoints) { + m_sedpAgent.addReader(*pReader); + } + return pReader; + } + } + + return nullptr; +} + +bool Participant::deleteReader(Reader *reader) { + std::lock_guard lock(m_mutex); + for (unsigned int i = 0; i < m_readers.size(); i++) { + if (m_readers[i]->getSEDPSequenceNumber() == + reader->getSEDPSequenceNumber()) { + if (m_sedpAgent.deleteReader(reader)) { + m_readers[i] = nullptr; + return true; + } + PARTICIPANT_LOG("Found reader but SEDP deletion failed"); + } + } + return false; +} + +bool Participant::deleteWriter(Writer *writer) { + std::lock_guard lock(m_mutex); + for (unsigned int i = 0; i < m_writers.size(); i++) { + if (m_writers[i]->getSEDPSequenceNumber() == + writer->getSEDPSequenceNumber()) { + if (m_sedpAgent.deleteWriter(writer)) { + m_writers[i] = nullptr; + return true; + } + PARTICIPANT_LOG("Found reader but SEDP deletion failed"); + } + } + return false; +} + +bool Participant::isReadersFull() { + std::lock_guard lock(m_mutex); + for (unsigned int i = 0; i < m_readers.size(); i++) { + if (m_readers[i] == nullptr) { + return false; + } + } + + return true; +} + +rtps::Writer *Participant::getWriter(EntityId_t id) { + std::lock_guard lock(m_mutex); + for (uint8_t i = 0; i < m_writers.size(); ++i) { + if (m_writers[i] == nullptr) { + continue; + } + if (m_writers[i]->m_attributes.endpointGuid.entityId == id) { + return m_writers[i]; + } + } + return nullptr; +} + +rtps::Reader *Participant::getReader(EntityId_t id) { + std::lock_guard lock(m_mutex); + for (uint8_t i = 0; i < m_readers.size(); ++i) { + if (m_readers[i] == nullptr) { + continue; + } + if (m_readers[i]->m_attributes.endpointGuid.entityId == id) { + return m_readers[i]; + } + } + return nullptr; +} + +rtps::Reader *Participant::getReaderByWriterId(const Guid_t &guid) { + std::lock_guard lock(m_mutex); + for (uint8_t i = 0; i < m_readers.size(); ++i) { + if (m_readers[i] == nullptr) { + continue; + } + if (m_readers[i]->isProxy(guid)) { + return m_readers[i]; + } + } + return nullptr; +} + +rtps::Writer *Participant::getMatchingWriter(const TopicData &readerTopicData) { + std::lock_guard lock(m_mutex); + for (uint8_t i = 0; i < m_writers.size(); ++i) { + if (m_writers[i] == nullptr) { + continue; + } + if (m_writers[i]->m_attributes.matchesTopicOf(readerTopicData) && + (readerTopicData.reliabilityKind == ReliabilityKind_t::BEST_EFFORT || + m_writers[i]->m_attributes.reliabilityKind == + ReliabilityKind_t::RELIABLE)) { + return m_writers[i]; + } + } + return nullptr; +} + +rtps::Reader *Participant::getMatchingReader(const TopicData &writerTopicData) { + std::lock_guard lock(m_mutex); + for (uint8_t i = 0; i < m_readers.size(); ++i) { + if (m_readers[i] == nullptr) { + continue; + } + if (m_readers[i]->m_attributes.matchesTopicOf(writerTopicData) && + (writerTopicData.reliabilityKind == ReliabilityKind_t::RELIABLE || + m_readers[i]->m_attributes.reliabilityKind == + ReliabilityKind_t::BEST_EFFORT)) { + return m_readers[i]; + } + } + return nullptr; +} + +rtps::Writer * +Participant::getMatchingWriter(const TopicDataCompressed &readerTopicData) { + std::lock_guard lock(m_mutex); + for (uint8_t i = 0; i < m_writers.size(); ++i) { + if (m_writers[i] == nullptr) { + continue; + } + if (readerTopicData.matchesTopicOf(m_writers[i]->m_attributes) && + (readerTopicData.is_reliable == false || + m_writers[i]->m_attributes.reliabilityKind == + ReliabilityKind_t::RELIABLE)) { + return m_writers[i]; + } + } + return nullptr; +} + +rtps::Reader * +Participant::getMatchingReader(const TopicDataCompressed &writerTopicData) { + std::lock_guard lock(m_mutex); + for (uint8_t i = 0; i < m_readers.size(); ++i) { + if (m_readers[i] == nullptr) { + continue; + } + if (writerTopicData.matchesTopicOf(m_readers[i]->m_attributes) && + (writerTopicData.is_reliable == true || + m_readers[i]->m_attributes.reliabilityKind == + ReliabilityKind_t::BEST_EFFORT)) { + return m_readers[i]; + } + } + return nullptr; +} + +bool Participant::addNewRemoteParticipant( + const ParticipantProxyData &remotePart) { + std::lock_guard lock(m_mutex); + return m_remoteParticipants.add(remotePart); +} + +bool Participant::removeRemoteParticipant(const GuidPrefix_t &prefix) { + std::lock_guard lock(m_mutex); + auto isElementToRemove = [&](const ParticipantProxyData &proxy) { + return proxy.m_guid.prefix == prefix; + }; + auto thunk = [](void *arg, const ParticipantProxyData &value) { + return (*static_cast(arg))(value); + }; + removeAllProxiesOfParticipant(prefix); + m_sedpAgent.removeUnmatchedEntitiesOfParticipant(prefix); + return m_remoteParticipants.remove(thunk, &isElementToRemove); +} + +void Participant::removeAllProxiesOfParticipant(const GuidPrefix_t &prefix) { + std::lock_guard lock(m_mutex); + for (unsigned int i = 0; i < m_readers.size(); i++) { + if (m_readers[i] == nullptr) { + continue; + } + m_readers[i]->removeAllProxiesOfParticipant(prefix); + } + + for (unsigned int i = 0; i < m_writers.size(); i++) { + if (m_writers[i] == nullptr) { + continue; + } + m_writers[i]->removeAllProxiesOfParticipant(prefix); + } +} + +void Participant::removeProxyFromAllEndpoints(const Guid_t &guid) { + std::lock_guard lock(m_mutex); + for (unsigned int i = 0; i < m_writers.size(); i++) { + if (m_writers[i] == nullptr) { + continue; + } + if (m_writers[i]->removeProxy(guid)) { + PARTICIPANT_LOG("Removing proxy for writer [{}, {}], proxies left = {}", + m_writers[i]->m_attributes.topicName, + m_writers[i]->m_attributes.typeName, + (int)m_writers[i]->getProxiesCount()); + } + } + + for (unsigned int i = 0; i < m_readers.size(); i++) { + if (m_readers[i] == nullptr) { + continue; + } + if (m_readers[i]->removeProxy(guid)) { + PARTICIPANT_LOG("Removing proxy for reader [{}, {}], proxies left = {}", + m_readers[i]->m_attributes.topicName, + m_readers[i]->m_attributes.typeName, + (int)m_readers[i]->getProxiesCount()); + } + } +} + +const rtps::ParticipantProxyData * +Participant::findRemoteParticipant(const GuidPrefix_t &prefix) { + std::lock_guard lock(m_mutex); + auto isElementToFind = [&](const ParticipantProxyData &proxy) { + return proxy.m_guid.prefix == prefix; + }; + auto thunk = [](void *arg, const ParticipantProxyData &value) { + return (*static_cast(arg))(value); + }; + return m_remoteParticipants.find(thunk, &isElementToFind); +} + +void Participant::refreshRemoteParticipantLiveliness( + const GuidPrefix_t &prefix) { + std::lock_guard lock(m_mutex); + auto isElementToFind = [&](const ParticipantProxyData &proxy) { + return proxy.m_guid.prefix == prefix; + }; + auto thunk = [](void *arg, const ParticipantProxyData &value) { + return (*static_cast(arg))(value); + }; + + auto remoteParticipant = m_remoteParticipants.find(thunk, &isElementToFind); + if (remoteParticipant != nullptr) { + remoteParticipant->onAliveSignal(); + } +} + +bool Participant::hasReaderWithMulticastLocator( + const std::array &address) { + std::lock_guard lock(m_mutex); + for (uint8_t i = 0; i < m_readers.size(); i++) { + if (m_readers[i] == nullptr) { + continue; + } + if (m_readers[i]->m_attributes.multicastLocator.getIp4AddressBytes() == + address) { + return true; + } + } + return false; +} + +uint32_t Participant::getRemoteParticipantCount() { + std::lock_guard lock(m_mutex); + return m_remoteParticipants.getNumElements(); +} + +rtps::MessageReceiver *Participant::getMessageReceiver() { return &m_receiver; } + +bool Participant::checkAndResetHeartbeats() { + std::lock_guard lock1(m_mutex); + std::lock_guard lock2(m_spdpAgent.m_mutex); + PARTICIPANT_LOG("Have {} remote participants", + (unsigned int)m_remoteParticipants.getNumElements()); + PARTICIPANT_LOG( + "Unmatched remote writers/readers, {} / {}", + static_cast(m_sedpAgent.getNumRemoteUnmatchedWriters()), + static_cast(m_sedpAgent.getNumRemoteUnmatchedReaders())); + for (auto &remote : m_remoteParticipants) { + PARTICIPANT_LOG("Remote GUID = {} {} {} {} | Age = {} [ms]", + remote.m_guid.prefix.id[4], remote.m_guid.prefix.id[5], remote.m_guid.prefix.id[6], remote.m_guid.prefix.id[7], (unsigned int)remote.getAliveSignalAgeInMilliseconds() ); + if (remote.isAlive()) { + continue; + } + PARTICIPANT_LOG("removing remote participant"); + bool success = removeRemoteParticipant(remote.m_guid.prefix); + if (!success) { + return false; + }else{ + return true; + } + } + return true; +} + +void Participant::printInfo() { + + uint32_t max_reader_proxies = 0; + for (unsigned int i = 0; i < m_readers.size(); i++) { + if (m_readers[i] != nullptr && m_readers[i]->isInitialized()) { + if (m_hasBuilInEndpoints && i < 3) { +#ifdef PARTICIPANT_PRINTINFO_LONG + if (m_readers[i]->m_attributes.endpointGuid.entityId == + ENTITYID_SPDP_BUILTIN_PARTICIPANT_READER) { + PARTICIPANT_LOG("Reader {}: SPDP BUILTIN READER | Remote Proxies = {}", + i, + static_cast(m_readers[i]->getProxiesCount())); + } + if (m_readers[i]->m_attributes.endpointGuid.entityId == + ENTITYID_SEDP_BUILTIN_PUBLICATIONS_READER) { + PARTICIPANT_LOG( + "Reader {}: SEDP PUBLICATION READER | Remote Proxies = {}", i, + static_cast(m_readers[i]->getProxiesCount())); + } + if (m_readers[i]->m_attributes.endpointGuid.entityId == + ENTITYID_SEDP_BUILTIN_SUBSCRIPTIONS_READER) { + PARTICIPANT_LOG( + "Reader {}: SEDP SUBSCRIPTION READER | Remote Proxies = {}", i, + static_cast(m_readers[i]->getProxiesCount())); + } +#endif + continue; + } + + max_reader_proxies = + std::max(max_reader_proxies, m_readers[i]->getProxiesCount()); +#ifdef PARTICIPANT_PRINTINFO_LONG + PARTICIPANT_LOG( + "Reader {}: Topic = {} | Type = {} | Remote Proxies = {} | SEDP " + "SN = {}", + i, m_readers[i]->m_attributes.topicName, + m_readers[i]->m_attributes.typeName, + static_cast(m_readers[i]->getProxiesCount()), + static_cast(m_readers[i]->getSEDPSequenceNumber().low)); +#endif + } + } + + uint32_t max_writer_proxies = 0; + for (unsigned int i = 0; i < m_writers.size(); i++) { + + if (m_hasBuilInEndpoints && i < 3) { +#ifdef PARTICIPANT_PRINTINFO_LONG + if (m_writers[i]->m_attributes.endpointGuid.entityId == + ENTITYID_SPDP_BUILTIN_PARTICIPANT_WRITER) { + PARTICIPANT_LOG("Writer {}: SPDP WRITER | Remote Proxies = {}", i, + static_cast(m_writers[i]->getProxiesCount())); + } + if (m_writers[i]->m_attributes.endpointGuid.entityId == + ENTITYID_SEDP_BUILTIN_PUBLICATIONS_WRITER) { + PARTICIPANT_LOG( + "Writer {}: SEDP PUBLICATION WRITER | Remote Proxies = {}", i, + static_cast(m_writers[i]->getProxiesCount())); + } + if (m_writers[i]->m_attributes.endpointGuid.entityId == + ENTITYID_SEDP_BUILTIN_SUBSCRIPTIONS_WRITER) { + PARTICIPANT_LOG( + "Writer {}: SEDP SUBSCRIPTION WRITER | Remote Proxies = {}", i, + static_cast(m_writers[i]->getProxiesCount())); + } +#endif + continue; + } + + if (m_writers[i] != nullptr && m_writers[i]->isInitialized()) { + max_writer_proxies = + std::max(max_writer_proxies, m_writers[i]->getProxiesCount()); +#ifdef PARTICIPANT_PRINTINFO_LONG + PARTICIPANT_LOG( + "Writer {}: Topic = {} | Type = {} | Remote Proxies = {} | SEDP " + "SN = {}", + i, m_writers[i]->m_attributes.topicName, + m_writers[i]->m_attributes.typeName, + static_cast(m_writers[i]->getProxiesCount()), + static_cast(m_writers[i]->getSEDPSequenceNumber().low)); +#endif + } + } + + PARTICIPANT_LOG("Max Writer Proxies {}", max_writer_proxies); + PARTICIPANT_LOG("Max Reader Proxies {}", max_reader_proxies); + PARTICIPANT_LOG("Unmatched Remote Readers = {}", + static_cast(m_sedpAgent.getNumRemoteUnmatchedReaders())); + PARTICIPANT_LOG("Unmatched Remote Writers = {}", + static_cast(m_sedpAgent.getNumRemoteUnmatchedWriters())); + PARTICIPANT_LOG("Remote Participants = {}", + static_cast(m_remoteParticipants.getNumElements())); +} + +rtps::SPDPAgent &Participant::getSPDPAgent() { return m_spdpAgent; } + +void Participant::addBuiltInEndpoints(BuiltInEndpoints &endpoints) { + std::lock_guard lock(m_mutex); + m_hasBuilInEndpoints = true; + m_spdpAgent.init(*this, endpoints); + m_sedpAgent.init(*this, endpoints); + + // This needs to be done after initializing the agents + addWriter(endpoints.spdpWriter); + addReader(endpoints.spdpReader); + addWriter(endpoints.sedpPubWriter); + addReader(endpoints.sedpPubReader); + addWriter(endpoints.sedpSubWriter); + addReader(endpoints.sedpSubReader); +} + +void Participant::newMessage(const uint8_t *data, DataSize_t size) { + if (!m_receiver.processMessage(data, size)) { + PARTICIPANT_LOG("MESSAGE PROCESSING FAILED"); + } +} diff --git a/components/rtps_embedded/src/entities/Reader.cpp b/components/rtps_embedded/src/entities/Reader.cpp new file mode 100644 index 000000000..1c7d64a28 --- /dev/null +++ b/components/rtps_embedded/src/entities/Reader.cpp @@ -0,0 +1,153 @@ +#include +#include +#include +#include +#include +#include + +using namespace rtps; + +Reader::Reader() + : espp::BaseComponent("RtpsReader", espp::Logger::Verbosity::WARN) { + m_callbacks.fill({nullptr, nullptr, 0}); +} + +void Reader::executeCallbacks(const ReaderCacheChange &cacheChange) { + std::lock_guard lock(m_callback_mutex); + for (unsigned int i = 0; i < m_callbacks.size(); i++) { + if (m_callbacks[i].function != nullptr) { + m_callbacks[i].function(m_callbacks[i].arg, cacheChange); + } + } +} + +bool Reader::initMutex() { + return true; +} + +void Reader::reset() { + std::lock_guard lock1(m_proxies_mutex); + std::lock_guard lock2(m_callback_mutex); + + m_proxies.clear(); + for (unsigned int i = 0; i < m_callbacks.size(); i++) { + m_callbacks[i].function = nullptr; + m_callbacks[i].arg = nullptr; + } + + m_callback_count = 0; + m_is_initialized_ = false; +} + +bool Reader::isProxy(const Guid_t &guid) { + for (const auto &proxy : m_proxies) { + if (proxy.remoteWriterGuid.operator==(guid)) { + return true; + } + } + return false; +} + +WriterProxy *Reader::getProxy(Guid_t guid) { + auto isElementToFind = [&](const WriterProxy &proxy) { + return proxy.remoteWriterGuid == guid; + }; + auto thunk = [](void *arg, const WriterProxy &value) { + return (*static_cast(arg))(value); + }; + return m_proxies.find(thunk, &isElementToFind); +} + +Reader::callbackIdentifier_t +Reader::registerCallback(Reader::callbackFunction_t cb, void *arg) { + std::lock_guard lock(m_callback_mutex); + if (m_callback_count == m_callbacks.size() || cb == nullptr) { + return false; + } + + for (unsigned int i = 0; i < m_callbacks.size(); i++) { + if (m_callbacks[i].function == nullptr) { + m_callbacks[i].function = cb; + m_callbacks[i].arg = arg; + m_callbacks[i].identifier = m_callback_identifier++; + m_callback_count++; + return m_callbacks[i].identifier; + } + } + + return 0; +} + +uint32_t Reader::getProxiesCount() { return m_proxies.getNumElements(); } + +bool Reader::removeCallback(Reader::callbackIdentifier_t identifier) { + std::lock_guard lock(m_callback_mutex); + for (unsigned int i = 0; i < m_callbacks.size(); i++) { + if (m_callbacks[i].identifier == identifier) { + m_callbacks[i].function = nullptr; + m_callbacks[i].arg = nullptr; + m_callback_count--; + return true; + } + } + + return false; +} + +uint8_t Reader::getNumCallbacks() { return m_callback_count; } + +void Reader::removeAllProxiesOfParticipant(const GuidPrefix_t &guidPrefix) { + std::lock_guard lock(m_proxies_mutex); + auto isElementToRemove = [&](const WriterProxy &proxy) { + return proxy.remoteWriterGuid.prefix == guidPrefix; + }; + auto thunk = [](void *arg, const WriterProxy &value) { + return (*static_cast(arg))(value); + }; + + m_proxies.remove(thunk, &isElementToRemove); +} + +bool Reader::removeProxy(const Guid_t &guid) { + std::lock_guard lock(m_proxies_mutex); + auto isElementToRemove = [&](const WriterProxy &proxy) { + return proxy.remoteWriterGuid == guid; + }; + auto thunk = [](void *arg, const WriterProxy &value) { + return (*static_cast(arg))(value); + }; + + return m_proxies.remove(thunk, &isElementToRemove); +} + +bool Reader::addNewMatchedWriter(const WriterProxy &newProxy) { + std::lock_guard lock(m_proxies_mutex); +#if (SFR_VERBOSE || SLR_VERBOSE) && RTPS_GLOBAL_VERBOSE + SFR_LOG("New writer added with id: "); + printGuid(newProxy.remoteWriterGuid); +#endif + return m_proxies.add(newProxy); +} + +void rtps::Reader::setSEDPSequenceNumber(const SequenceNumber_t &sn) { + m_sedp_sequence_number = sn; +} +const rtps::SequenceNumber_t &rtps::Reader::getSEDPSequenceNumber() { + return m_sedp_sequence_number; +} + +int rtps::Reader::dumpAllProxies(dumpProxyCallback target, void *arg) { + if (target == nullptr) { + return 0; + } + std::lock_guard lock(m_proxies_mutex); + int dump_count = 0; + for (auto it = m_proxies.begin(); it != m_proxies.end(); ++it, ++dump_count) { + target(this, *it, arg); + } + return dump_count; +} + +bool rtps::Reader::sendPreemptiveAckNack(const WriterProxy &writer) { + return true; +} diff --git a/components/rtps_embedded/src/entities/StatelessReader.cpp b/components/rtps_embedded/src/entities/StatelessReader.cpp new file mode 100644 index 000000000..f2ec862e4 --- /dev/null +++ b/components/rtps_embedded/src/entities/StatelessReader.cpp @@ -0,0 +1,80 @@ +/* +The MIT License +Copyright (c) 2019 Lehrstuhl Informatik 11 - RWTH Aachen University +Modifications Copyright (c) 2026 ATDev +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: +The above copyright notice and this permission notice shall be included in +all copies or substantial portions of the Software. +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +THE SOFTWARE + +This file is part of embeddedRTPS. + +Author: i11 - Embedded Software, RWTH Aachen University +*/ + +#include "rtps/entities/StatelessReader.h" +#include "rtps/utils/Log.h" + +using rtps::StatelessReader; + +#if SLR_VERBOSE && RTPS_GLOBAL_VERBOSE +#include "rtps/utils/printutils.h" +#define SLR_LOG(...) \ + if (true) { \ + printf("[StatelessReader %s] ", &m_attributes.topicName[0]); \ + printf(__VA_ARGS__); \ + printf("\n"); \ + } +#else +#define SLR_LOG(...) // +#endif + +bool StatelessReader::init(const TopicData &attributes) { + if (!initMutex()) { + return false; + } + + m_proxies.clear(); + m_attributes = attributes; + m_is_initialized_ = true; + return true; +} + +void StatelessReader::newChange(const ReaderCacheChange &cacheChange) { + if (!m_is_initialized_) { + return; + } + executeCallbacks(cacheChange); +} + +bool StatelessReader::addNewMatchedWriter(const WriterProxy &newProxy) { +#if (SLR_VERBOSE && RTPS_GLOBAL_VERBOSE) + SLR_LOG("Adding WriterProxy"); + printGuid(newProxy.remoteWriterGuid); +#endif + return m_proxies.add(newProxy); +} + +bool StatelessReader::onNewHeartbeat(const SubmessageHeartbeat &, + const GuidPrefix_t &) { + // nothing to do + return true; +} + +bool StatelessReader::onNewGapMessage(const SubmessageGap &msg, + const GuidPrefix_t &remotePrefix) { + return true; +} + +#undef SLR_VERBOSE diff --git a/components/rtps_embedded/src/entities/Writer.cpp b/components/rtps_embedded/src/entities/Writer.cpp new file mode 100644 index 000000000..43a5e3838 --- /dev/null +++ b/components/rtps_embedded/src/entities/Writer.cpp @@ -0,0 +1,151 @@ +#include "rtps/utils/Log.h" +#include +#include +#include +#include +#include +#include + +using namespace rtps; + +Writer::Writer() + : espp::BaseComponent("RtpsWriter", espp::Logger::Verbosity::WARN) {} + +bool rtps::Writer::addNewMatchedReader(const ReaderProxy &newProxy) { + INIT_GUARD(); +#if SFW_VERBOSE && RTPS_GLOBAL_VERBOSE + SFW_LOG("New reader added with id: "); + printGuid(newProxy.remoteReaderGuid); +#endif + std::lock_guard lock(m_mutex); + bool success = m_proxies.add(newProxy); + if (!m_enforceUnicast) { + manageSendOptions(); + } + return success; +} + +bool rtps::Writer::removeProxy(const Guid_t &guid) { + INIT_GUARD() + std::lock_guard lock(m_mutex); + auto isElementToRemove = [&](const ReaderProxy &proxy) { + return proxy.remoteReaderGuid == guid; + }; + auto thunk = [](void *arg, const ReaderProxy &value) { + return (*static_cast(arg))(value); + }; + + bool ret = m_proxies.remove(thunk, &isElementToRemove); + resetSendOptions(); + return ret; +} + +uint32_t rtps::Writer::getProxiesCount() { + std::lock_guard lock(m_mutex); + return m_proxies.getNumElements(); +} + +void rtps::Writer::resetSendOptions() { + INIT_GUARD() + for (auto &proxy : m_proxies) { + proxy.suppressUnicast = false; + proxy.useMulticast = false; + proxy.unknown_eid = false; + } + manageSendOptions(); +} + +const rtps::CacheChange *rtps::Writer::newChange(ChangeKind_t kind, + const uint8_t *data, + DataSize_t size) { + return newChange(kind, data, size, false, false); +} + +void rtps::Writer::manageSendOptions() { + INIT_GUARD(); + std::lock_guard lock(m_mutex); + for (auto &proxy : m_proxies) { + if (proxy.remoteMulticastLocator.kind == + LocatorKind_t::LOCATOR_KIND_INVALID) { + proxy.suppressUnicast = false; + proxy.useMulticast = false; + } else { + bool found = false; + for (auto &avproxy : m_proxies) { + if (avproxy.remoteMulticastLocator.kind == + LocatorKind_t::LOCATOR_KIND_UDPv4 && + avproxy.remoteMulticastLocator.getIp4AddressBytes() == + proxy.remoteMulticastLocator.getIp4AddressBytes() && + avproxy.remoteLocator.getIp4AddressBytes() != + proxy.remoteLocator.getIp4AddressBytes()) { + if (avproxy.suppressUnicast == false) { + avproxy.useMulticast = false; + avproxy.suppressUnicast = true; + proxy.useMulticast = true; + proxy.suppressUnicast = true; + if (avproxy.remoteReaderGuid.entityId != + proxy.remoteReaderGuid.entityId) { + proxy.unknown_eid = true; + } + } + found = true; + } + } + if (!found) { + proxy.useMulticast = false; + proxy.suppressUnicast = false; + } + } + } +} + +void rtps::Writer::removeAllProxiesOfParticipant( + const GuidPrefix_t &guidPrefix) { + INIT_GUARD(); + std::lock_guard lock(m_mutex); + auto isElementToRemove = [&](const ReaderProxy &proxy) { + return proxy.remoteReaderGuid.prefix == guidPrefix; + }; + auto thunk = [](void *arg, const ReaderProxy &value) { + return (*static_cast(arg))(value); + }; + + m_proxies.remove(thunk, &isElementToRemove); + resetSendOptions(); +} + +bool rtps::Writer::isBuiltinEndpoint() { + return !(m_attributes.endpointGuid.entityId.entityKind == + EntityKind_t::USER_DEFINED_WRITER_WITHOUT_KEY || + m_attributes.endpointGuid.entityId.entityKind == + EntityKind_t::USER_DEFINED_WRITER_WITH_KEY); +} + +bool rtps::Writer::isIrrelevant(ChangeKind_t kind) const { + // Right now we only allow alive changes + // return kind == ChangeKind_t::INVALID || (m_topicKind == TopicKind_t::NO_KEY + // && kind != ChangeKind_t::ALIVE); + return kind != ChangeKind_t::ALIVE; +} + +bool rtps::Writer::isInitialized() { return m_is_initialized_; } + +void rtps::Writer::setSEDPSequenceNumber(const SequenceNumber_t &sn) { + m_sedp_sequence_number = sn; +} + +const rtps::SequenceNumber_t &rtps::Writer::getSEDPSequenceNumber() { + return m_sedp_sequence_number; +} + +int rtps::Writer::dumpAllProxies(dumpProxyCallback target, void *arg) { + if (target == nullptr) { + return 0; + } + std::lock_guard lock(m_mutex); + int dump_count = 0; + for (auto it = m_proxies.begin(); it != m_proxies.end(); ++it, ++dump_count) { + target(this, *it, arg); + } + return dump_count; +} diff --git a/components/rtps_embedded/src/messages/MessageReceiver.cpp b/components/rtps_embedded/src/messages/MessageReceiver.cpp new file mode 100644 index 000000000..994927513 --- /dev/null +++ b/components/rtps_embedded/src/messages/MessageReceiver.cpp @@ -0,0 +1,234 @@ +/* +The MIT License +Copyright (c) 2019 Lehrstuhl Informatik 11 - RWTH Aachen University +Modifications Copyright (c) 2026 ATDev +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: +The above copyright notice and this permission notice shall be included in +all copies or substantial portions of the Software. +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +THE SOFTWARE + +This file is part of embeddedRTPS. + +Author: i11 - Embedded Software, RWTH Aachen University +*/ + +#include "rtps/messages/MessageReceiver.h" +#include + +#include "rtps/entities/Reader.h" +#include "rtps/entities/Writer.h" +#include "rtps/messages/MessageTypes.h" +#include "rtps/utils/Log.h" + +using rtps::MessageReceiver; + +#if RECV_VERBOSE && RTPS_GLOBAL_VERBOSE +#include "rtps/utils/printutils.h" +#define RECV_LOG(...) logger_.warn(__VA_ARGS__) +#else +#define RECV_LOG(...) do { } while (0) +#endif + +MessageReceiver::MessageReceiver(Participant *part) + : espp::BaseComponent("RtpsMessageReceiver", espp::Logger::Verbosity::WARN), + mp_part(part) {} + +void MessageReceiver::resetState() { + sourceGuidPrefix = GUIDPREFIX_UNKNOWN; + sourceVersion = PROTOCOLVERSION; + sourceVendor = VENDOR_UNKNOWN; + haveTimeStamp = false; +} + +bool MessageReceiver::processMessage(const uint8_t *data, DataSize_t size) { + resetState(); + MessageProcessingInfo msgInfo(data, size); + + if (!processHeader(msgInfo)) { + return false; + } + SubmessageHeader submsgHeader; + while (msgInfo.nextPos < msgInfo.size) { + if (!deserializeMessage(msgInfo, submsgHeader)) { + return false; + } + processSubmessage(msgInfo, submsgHeader); + } + + return true; +} + +bool MessageReceiver::processHeader(MessageProcessingInfo &msgInfo) { + Header header; + if (!deserializeMessage(msgInfo, header)) { + return false; + } + + if (header.guidPrefix.id == mp_part->m_guidPrefix.id) { + RECV_LOG("[MessageReceiver]: Received own message."); + return false; // Don't process our own packet + } + + if (header.protocolName != RTPS_PROTOCOL_NAME || + header.protocolVersion.major != PROTOCOLVERSION.major) { + return false; + } + + sourceGuidPrefix = header.guidPrefix; + sourceVendor = header.vendorId; + sourceVersion = header.protocolVersion; + + msgInfo.nextPos += Header::getRawSize(); + return true; +} + +bool MessageReceiver::processSubmessage(MessageProcessingInfo &msgInfo, + const SubmessageHeader &submsgHeader) { + bool success = false; + + switch (submsgHeader.submessageId) { + case SubmessageKind::ACKNACK: + RECV_LOG("Processing AckNack submessage"); + success = processAckNackSubmessage(msgInfo); + break; + case SubmessageKind::DATA: + RECV_LOG("Processing Data submessage"); + success = processDataSubmessage(msgInfo, submsgHeader); + break; + case SubmessageKind::HEARTBEAT: + RECV_LOG("Processing Heartbeat submessage"); + success = processHeartbeatSubmessage(msgInfo); + break; + case SubmessageKind::INFO_DST: + RECV_LOG("Info_DST submessage not relevant."); + success = true; // Not relevant + break; + case SubmessageKind::GAP: + RECV_LOG("Processing GAP submessage"); + success = processGapSubmessage(msgInfo); + break; + case SubmessageKind::INFO_TS: + RECV_LOG("Info_TS submessage not relevant."); + success = true; // Not relevant now + break; + default: + RECV_LOG("Submessage of type {} currently not supported. Skipping..", + static_cast(submsgHeader.submessageId)); + success = false; + } + msgInfo.nextPos += + submsgHeader.octetsToNextHeader + SubmessageHeader::getRawSize(); + return success; +} + +bool MessageReceiver::processDataSubmessage( + MessageProcessingInfo &msgInfo, const SubmessageHeader &submsgHeader) { + SubmessageData dataSubmsg; + if (!deserializeMessage(msgInfo, dataSubmsg)) { + return false; + } + + const uint8_t *serializedData = + msgInfo.getPointerToCurrentPos() + SubmessageData::getRawSize(); + + const DataSize_t size = submsgHeader.octetsToNextHeader - + SubmessageData::getRawSize() + + SubmessageHeader::getRawSize(); + + RECV_LOG("Received data message size {}", static_cast(size)); + + Reader *reader; + if (dataSubmsg.readerId == ENTITYID_UNKNOWN) { +#if RECV_VERBOSE && RTPS_GLOBAL_VERBOSE + RECV_LOG("Received ENTITYID_UNKNOWN readerID, searching for writer ID = "); + printGuid(Guid_t{sourceGuidPrefix, dataSubmsg.writerId}); +#endif + reader = mp_part->getReaderByWriterId( + Guid_t{sourceGuidPrefix, dataSubmsg.writerId}); + if (reader != nullptr) + RECV_LOG("Found reader!"); + } else { + reader = mp_part->getReader(dataSubmsg.readerId); +#if RECV_VERBOSE && RTPS_GLOBAL_VERBOSE + auto reader_by_writer = mp_part->getReaderByWriterId( + Guid_t{sourceGuidPrefix, dataSubmsg.writerId}); + + if (reader_by_writer == nullptr && reader != nullptr) { + RECV_LOG("FOUND By READER ID, NOT BY WRITER ID ="); + printGuid(Guid_t{sourceGuidPrefix, dataSubmsg.writerId}); + } +#endif + } + if (reader != nullptr) { + Guid_t writerGuid{sourceGuidPrefix, dataSubmsg.writerId}; + ReaderCacheChange change{ChangeKind_t::ALIVE, writerGuid, + dataSubmsg.writerSN, serializedData, size}; + reader->newChange(change); + } else { +#if RECV_VERBOSE && RTPS_GLOBAL_VERBOSE + RECV_LOG("Couldn't find a reader with id: "); + printEntityId(dataSubmsg.readerId); +#endif + } + + return true; +} + +bool MessageReceiver::processHeartbeatSubmessage( + MessageProcessingInfo &msgInfo) { + SubmessageHeartbeat submsgHB; + if (!deserializeMessage(msgInfo, submsgHB)) { + return false; + } + + Reader *reader = mp_part->getReader(submsgHB.readerId); + if (reader != nullptr) { + reader->onNewHeartbeat(submsgHB, sourceGuidPrefix); + mp_part->refreshRemoteParticipantLiveliness(sourceGuidPrefix); + return true; + } else { + return false; + } +} + +bool MessageReceiver::processAckNackSubmessage(MessageProcessingInfo &msgInfo) { + SubmessageAckNack submsgAckNack; + if (!deserializeMessage(msgInfo, submsgAckNack)) { + return false; + } + + Writer *writer = mp_part->getWriter(submsgAckNack.writerId); + if (writer != nullptr) { + writer->onNewAckNack(submsgAckNack, sourceGuidPrefix); + return true; + } else { + return false; + } +} + +bool MessageReceiver::processGapSubmessage(MessageProcessingInfo &msgInfo) { + SubmessageGap submsgGap; + if (!deserializeMessage(msgInfo, submsgGap)) { + return false; + } + + Reader *reader = mp_part->getReader(submsgGap.readerId); + if (reader != nullptr) { + reader->onNewGapMessage(submsgGap, sourceGuidPrefix); + return true; + } else { + return false; + } +} +#undef RECV_VERBOSE diff --git a/components/rtps_embedded/src/messages/MessageTypes.cpp b/components/rtps_embedded/src/messages/MessageTypes.cpp new file mode 100644 index 000000000..0a9bfb174 --- /dev/null +++ b/components/rtps_embedded/src/messages/MessageTypes.cpp @@ -0,0 +1,222 @@ +/* +The MIT License +Copyright (c) 2019 Lehrstuhl Informatik 11 - RWTH Aachen University +Modifications Copyright (c) 2026 ATDev +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: +The above copyright notice and this permission notice shall be included in +all copies or substantial portions of the Software. +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +THE SOFTWARE + +This file is part of embeddedRTPS. + +Author: i11 - Embedded Software, RWTH Aachen University +*/ + +#include "rtps/messages/MessageTypes.h" + +#include +#include + +#include +using namespace rtps; + +void doCopyAndMoveOn(uint8_t *dst, const uint8_t *&src, size_t size) { + memcpy(dst, src, size); + src += size; +} + +bool rtps::deserializeMessage(const MessageProcessingInfo &info, + Header &header) { + if (info.getRemainingSize() < Header::getRawSize()) { + return false; + } + + const uint8_t *currentPos = info.getPointerToCurrentPos(); + doCopyAndMoveOn(header.protocolName.data(), currentPos, + sizeof(std::array)); + doCopyAndMoveOn(reinterpret_cast(&header.protocolVersion), + currentPos, sizeof(ProtocolVersion_t)); + doCopyAndMoveOn(header.vendorId.vendorId.data(), currentPos, + header.vendorId.vendorId.size()); + doCopyAndMoveOn(header.guidPrefix.id.data(), currentPos, + header.guidPrefix.id.size()); + return true; +} + +bool rtps::deserializeMessage(const MessageProcessingInfo &info, + SubmessageHeader &header) { + if (info.getRemainingSize() < SubmessageHeader::getRawSize()) { + return false; + } + + const uint8_t *currentPos = info.getPointerToCurrentPos(); + header.submessageId = static_cast(*currentPos++); + header.flags = *(currentPos++); + doCopyAndMoveOn(reinterpret_cast(&header.octetsToNextHeader), + currentPos, sizeof(uint16_t)); + return true; +} + +bool rtps::deserializeMessage(const MessageProcessingInfo &info, + SubmessageData &msg) { + if (info.getRemainingSize() < SubmessageHeader::getRawSize()) { + return false; + } + if (!deserializeMessage(info, msg.header)) { + return false; + } + + // Check for length including data + if (info.getRemainingSize() < + SubmessageHeader::getRawSize() + msg.header.octetsToNextHeader) { + return false; + } + + const uint8_t *currentPos = + info.getPointerToCurrentPos() + SubmessageHeader::getRawSize(); + + doCopyAndMoveOn(reinterpret_cast(&msg.extraFlags), currentPos, + sizeof(uint16_t)); + doCopyAndMoveOn(reinterpret_cast(&msg.octetsToInlineQos), + currentPos, sizeof(uint16_t)); + doCopyAndMoveOn(msg.readerId.entityKey.data(), currentPos, + msg.readerId.entityKey.size()); + msg.readerId.entityKind = static_cast(*currentPos++); + doCopyAndMoveOn(msg.writerId.entityKey.data(), currentPos, + msg.writerId.entityKey.size()); + msg.writerId.entityKind = static_cast(*currentPos++); + doCopyAndMoveOn(reinterpret_cast(&msg.writerSN.high), currentPos, + sizeof(msg.writerSN.high)); + doCopyAndMoveOn(reinterpret_cast(&msg.writerSN.low), currentPos, + sizeof(msg.writerSN.low)); + return true; +} + +bool rtps::deserializeMessage(const MessageProcessingInfo &info, + SubmessageHeartbeat &msg) { + if (info.getRemainingSize() < SubmessageHeartbeat::getRawSize()) { + return false; + } + if (!deserializeMessage(info, msg.header)) { + return false; + } + + const uint8_t *currentPos = + info.getPointerToCurrentPos() + SubmessageHeader::getRawSize(); + + doCopyAndMoveOn(msg.readerId.entityKey.data(), currentPos, + msg.readerId.entityKey.size()); + msg.readerId.entityKind = static_cast(*currentPos++); + doCopyAndMoveOn(msg.writerId.entityKey.data(), currentPos, + msg.writerId.entityKey.size()); + msg.writerId.entityKind = static_cast(*currentPos++); + doCopyAndMoveOn(reinterpret_cast(&msg.firstSN.high), currentPos, + sizeof(msg.firstSN.high)); + doCopyAndMoveOn(reinterpret_cast(&msg.firstSN.low), currentPos, + sizeof(msg.firstSN.low)); + doCopyAndMoveOn(reinterpret_cast(&msg.lastSN.high), currentPos, + sizeof(msg.lastSN.high)); + doCopyAndMoveOn(reinterpret_cast(&msg.lastSN.low), currentPos, + sizeof(msg.lastSN.low)); + doCopyAndMoveOn(reinterpret_cast(&msg.count.value), currentPos, + sizeof(msg.count.value)); + return true; +} + +void rtps::deserializeSNS(const uint8_t *&position, SequenceNumberSet &set, + std::size_t num_bitfields) { + + doCopyAndMoveOn(reinterpret_cast(&set.base.high), position, + sizeof(SequenceNumber_t::high)); + doCopyAndMoveOn(reinterpret_cast(&set.base.low), position, + sizeof(SequenceNumber_t::low)); + doCopyAndMoveOn(reinterpret_cast(&set.numBits), position, + sizeof(uint32_t)); + + // Ensure that we copy not more bits than our sequence number set can hold + if (set.numBits != 0) { + // equal to size = std::min(SNS_NUM_BYTES, num_bitfields) + std::size_t size = + num_bitfields > SNS_NUM_BYTES ? SNS_NUM_BYTES : num_bitfields; + doCopyAndMoveOn(reinterpret_cast(set.bitMap.data()), position, + size); + position += (num_bitfields - size); + } +} + +bool rtps::deserializeMessage(const MessageProcessingInfo &info, + SubmessageAckNack &msg) { + const DataSize_t remainingSizeAtBeginning = info.getRemainingSize(); + if (remainingSizeAtBeginning < + SubmessageAckNack:: + getRawSizeWithoutSNSet()) { // Size of SequenceNumberSet unknown + return false; + } + if (!deserializeMessage(info, msg.header)) { + return false; + } + + const uint8_t *currentPos = + info.getPointerToCurrentPos() + SubmessageHeader::getRawSize(); + + doCopyAndMoveOn(msg.readerId.entityKey.data(), currentPos, + msg.readerId.entityKey.size()); + msg.readerId.entityKind = static_cast(*currentPos++); + doCopyAndMoveOn(msg.writerId.entityKey.data(), currentPos, + msg.writerId.entityKey.size()); + msg.writerId.entityKind = static_cast(*currentPos++); + + std::size_t num_bitfields = + msg.header.octetsToNextHeader - 4 - 4 - 8 - 4 - 4; + deserializeSNS(currentPos, msg.readerSNState, num_bitfields); + + doCopyAndMoveOn(reinterpret_cast(&msg.count.value), currentPos, + sizeof(msg.count.value)); + return true; +} + +bool rtps::deserializeMessage(const MessageProcessingInfo &info, + SubmessageGap &msg) { + + const DataSize_t remainingSizeAtBeginning = info.getRemainingSize(); + if (remainingSizeAtBeginning < + SubmessageGap::getRawSizeWithoutSNSet()) { // Size of SequenceNumberSet + // unknown + return false; + } + if (!deserializeMessage(info, msg.header)) { + return false; + } + + const uint8_t *currentPos = + info.getPointerToCurrentPos() + SubmessageHeader::getRawSize(); + + doCopyAndMoveOn(msg.readerId.entityKey.data(), currentPos, + msg.readerId.entityKey.size()); + msg.readerId.entityKind = static_cast(*currentPos++); + doCopyAndMoveOn(msg.writerId.entityKey.data(), currentPos, + msg.writerId.entityKey.size()); + msg.writerId.entityKind = static_cast(*currentPos++); + + doCopyAndMoveOn(reinterpret_cast(&msg.gapStart.high), currentPos, + sizeof(msg.gapStart.high)); + doCopyAndMoveOn(reinterpret_cast(&msg.gapStart.low), currentPos, + sizeof(msg.gapStart.low)); + + std::size_t num_bitfields = + msg.header.octetsToNextHeader - 4 - 4 - 8 - 8 - 4; + deserializeSNS(currentPos, msg.gapList, num_bitfields); + + return true; +} diff --git a/components/rtps_embedded/src/utils/Diagnostics.cpp b/components/rtps_embedded/src/utils/Diagnostics.cpp new file mode 100644 index 000000000..2f50f0c1d --- /dev/null +++ b/components/rtps_embedded/src/utils/Diagnostics.cpp @@ -0,0 +1,53 @@ +#include + +namespace rtps { +namespace Diagnostics { + +namespace ThreadPool { +uint32_t dropped_incoming_packets_usertraffic = 0; +uint32_t dropped_incoming_packets_metatraffic = 0; + +uint32_t dropped_outgoing_packets_usertraffic = 0; +uint32_t dropped_outgoing_packets_metatraffic = 0; + +uint32_t processed_incoming_metatraffic = 0; +uint32_t processed_outgoing_metatraffic = 0; +uint32_t processed_incoming_usertraffic = 0; +uint32_t processed_outgoing_usertraffic = 0; + +uint32_t max_ever_elements_outgoing_usertraffic_queue; +uint32_t max_ever_elements_incoming_usertraffic_queue; + +uint32_t max_ever_elements_outgoing_metatraffic_queue; +uint32_t max_ever_elements_incoming_metatraffic_queue; + +} // namespace ThreadPool + +namespace StatefulReader { +uint32_t sfr_unexpected_sn; +uint32_t sfr_retransmit_requests; +} // namespace StatefulReader + +namespace Network { +uint32_t lwip_allocation_failures; +} + +namespace SEDP { +uint32_t max_ever_remote_participants; +uint32_t current_remote_participants; + +uint32_t max_ever_matched_reader_proxies; +uint32_t current_max_matched_reader_proxies; + +uint32_t max_ever_matched_writer_proxies; +uint32_t current_max_matched_writer_proxies; + +uint32_t max_ever_unmatched_reader_proxies; +uint32_t current_max_unmatched_reader_proxies; + +uint32_t max_ever_unmatched_writer_proxies; +uint32_t current_max_unmatched_writer_proxies; +} // namespace SEDP + +} // namespace Diagnostics +} // namespace rtps diff --git a/components/rtps_embedded/thirdparty/Micro-CDR b/components/rtps_embedded/thirdparty/Micro-CDR new file mode 160000 index 000000000..550f87de8 --- /dev/null +++ b/components/rtps_embedded/thirdparty/Micro-CDR @@ -0,0 +1 @@ +Subproject commit 550f87de804edf2ad6682e81868dadb6a375b9e0 diff --git a/components/socket/CMakeLists.txt b/components/socket/CMakeLists.txt index 5bc6ad34a..6c90d2f4e 100644 --- a/components/socket/CMakeLists.txt +++ b/components/socket/CMakeLists.txt @@ -1,4 +1,4 @@ idf_component_register( INCLUDE_DIRS "include" SRC_DIRS "src" - REQUIRES base_component task lwip) + REQUIRES base_component task ) diff --git a/components/thread_pool/CMakeLists.txt b/components/thread_pool/CMakeLists.txt new file mode 100644 index 000000000..cb1e7ccab --- /dev/null +++ b/components/thread_pool/CMakeLists.txt @@ -0,0 +1,4 @@ +idf_component_register( + INCLUDE_DIRS "include" + SRC_DIRS "src" + REQUIRES base_component task) diff --git a/components/thread_pool/README.md b/components/thread_pool/README.md new file mode 100644 index 000000000..271e12ce7 --- /dev/null +++ b/components/thread_pool/README.md @@ -0,0 +1,13 @@ +# Thread Pool Component + +The `ThreadPool` component provides a reusable pool of worker tasks for executing queued jobs asynchronously. + +It is implemented with `espp::Task` workers and `std::condition_variable` synchronization. + +## Features + +- Configurable worker count +- Bounded or unbounded queue +- Optional blocking submit mode for backpressure +- Graceful stop (drains queued jobs) +- Thread-safe stats for submitted / executed / rejected jobs diff --git a/components/thread_pool/example/CMakeLists.txt b/components/thread_pool/example/CMakeLists.txt new file mode 100644 index 000000000..d480eb92c --- /dev/null +++ b/components/thread_pool/example/CMakeLists.txt @@ -0,0 +1,22 @@ +# The following lines of boilerplate have to be in your project's CMakeLists +# in this exact order for cmake to work correctly +cmake_minimum_required(VERSION 3.20) + +set(ENV{IDF_COMPONENT_MANAGER} "0") +include($ENV{IDF_PATH}/tools/cmake/project.cmake) + +# add the component directories that we want to use +set(EXTRA_COMPONENT_DIRS + "../../../components/" +) + +set( + COMPONENTS + "main esptool_py thread_pool" + CACHE STRING + "List of components to include" + ) + +project(thread_pool_example) + +set(CMAKE_CXX_STANDARD 20) diff --git a/components/thread_pool/example/README.md b/components/thread_pool/example/README.md new file mode 100644 index 000000000..4c0790c06 --- /dev/null +++ b/components/thread_pool/example/README.md @@ -0,0 +1,17 @@ +# Thread Pool Example + +This example shows how to: + +- Create an `espp::ThreadPool` +- Submit jobs +- Wait for all jobs to complete +- Read execution statistics + +## Build + +From this directory, run: + +```bash +idf.py set-target esp32 +idf.py build flash monitor +``` diff --git a/components/thread_pool/example/main/CMakeLists.txt b/components/thread_pool/example/main/CMakeLists.txt new file mode 100644 index 000000000..a941e22ba --- /dev/null +++ b/components/thread_pool/example/main/CMakeLists.txt @@ -0,0 +1,2 @@ +idf_component_register(SRC_DIRS "." + INCLUDE_DIRS ".") diff --git a/components/thread_pool/example/main/thread_pool_example.cpp b/components/thread_pool/example/main/thread_pool_example.cpp new file mode 100644 index 000000000..4191e5b25 --- /dev/null +++ b/components/thread_pool/example/main/thread_pool_example.cpp @@ -0,0 +1,67 @@ +#include +#include +#include +#include +#include + +#include "logger.hpp" +#include "thread_pool.hpp" + +using namespace std::chrono_literals; + +extern "C" void app_main(void) { + espp::Logger logger({.tag = "ThreadPool Example", .level = espp::Logger::Verbosity::INFO}); + + std::mutex done_mutex; + std::condition_variable done_cv; + constexpr int total_jobs = 8; + std::atomic completed_jobs = 0; + + espp::ThreadPool pool({ + .worker_count = 2, + .max_queue_size = 16, + .auto_start = true, + .block_on_submit_when_full = false, + .worker_task_config = { + .name = "tp_worker", + .stack_size_bytes = 4096, + .priority = 5, + .core_id = -1, + }, + .log_level = espp::Logger::Verbosity::WARN, + }); + + logger.info("Submitting {} jobs", total_jobs); + + for (int i = 0; i < total_jobs; ++i) { + bool queued = pool.submit([&, i]() { + std::this_thread::sleep_for(100ms); + int done = ++completed_jobs; + logger.info("Job {} done ({}/{})", i, done, total_jobs); + if (done == total_jobs) { + std::lock_guard lock(done_mutex); + done_cv.notify_one(); + } + }); + + if (!queued) { + logger.error("Failed to queue job {}", i); + } + } + + { + std::unique_lock lock(done_mutex); + done_cv.wait(lock, [&]() { return completed_jobs.load() == total_jobs; }); + } + + auto stats = pool.stats(); + logger.info("ThreadPool stats: submitted={} executed={} rejected={}", stats.submitted, + stats.executed, stats.rejected); + + pool.stop(); + logger.info("ThreadPool example complete"); + + while (true) { + std::this_thread::sleep_for(1s); + } +} diff --git a/components/thread_pool/example/sdkconfig.defaults b/components/thread_pool/example/sdkconfig.defaults new file mode 100644 index 000000000..75ebeae1c --- /dev/null +++ b/components/thread_pool/example/sdkconfig.defaults @@ -0,0 +1 @@ +CONFIG_COMPILER_CXX_EXCEPTIONS=y diff --git a/components/thread_pool/idf_component.yml b/components/thread_pool/idf_component.yml new file mode 100644 index 000000000..71a535f71 --- /dev/null +++ b/components/thread_pool/idf_component.yml @@ -0,0 +1,21 @@ +## IDF Component Manager Manifest File +license: "MIT" +description: "Generic thread pool component implemented with espp::Task and condition variables." +url: "https://github.com/esp-cpp/espp/tree/main/components/thread_pool" +repository: "git://github.com/esp-cpp/espp.git" +maintainers: + - William Emfinger +documentation: "https://esp-cpp.github.io/espp/core/thread_pool.html" +examples: + - path: example +tags: + - cpp + - Component + - ThreadPool + - Task + - Concurrent +dependencies: + idf: + version: '>=5.0' + espp/base_component: '>=1.0' + espp/task: '>=1.0' diff --git a/components/thread_pool/include/thread_pool.hpp b/components/thread_pool/include/thread_pool.hpp new file mode 100644 index 000000000..b2f903c57 --- /dev/null +++ b/components/thread_pool/include/thread_pool.hpp @@ -0,0 +1,81 @@ +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "base_component.hpp" +#include "task.hpp" + +namespace espp { + +class ThreadPool : public espp::BaseComponent { +public: + using Job = std::function; + + struct Stats { + std::uint64_t submitted = 0; + std::uint64_t executed = 0; + std::uint64_t rejected = 0; + }; + + struct Config { + std::size_t worker_count = 1; + std::size_t max_queue_size = 0; // 0 means unbounded + bool auto_start = true; + bool block_on_submit_when_full = false; + espp::Task::BaseConfig worker_task_config = { + .name = "thread_pool_worker", + .stack_size_bytes = 4096, + .priority = 5, + .core_id = -1, + }; + espp::Logger::Verbosity log_level = espp::Logger::Verbosity::WARN; + }; + + explicit ThreadPool(const Config &config); + ~ThreadPool(); + + void start(); + void stop(); + + bool is_running() const; + + bool submit(const Job &job); + bool try_submit(const Job &job); + + std::size_t queue_size() const; + std::size_t worker_count() const; + + Stats stats() const; + +private: + bool worker_task_fn(std::mutex &task_mutex, std::condition_variable &task_cv, + bool &task_notified); + + bool submit_impl(const Job &job, bool allow_blocking_when_full); + + Config config_; + + mutable std::mutex queue_mutex_; + std::condition_variable queue_has_work_cv_; + std::condition_variable queue_has_space_cv_; + std::deque queue_; + + std::vector> workers_; + std::atomic running_{false}; + bool stopping_ = false; + + std::atomic submitted_{0}; + std::atomic executed_{0}; + std::atomic rejected_{0}; +}; + +} // namespace espp diff --git a/components/thread_pool/src/thread_pool.cpp b/components/thread_pool/src/thread_pool.cpp new file mode 100644 index 000000000..55b6d2faa --- /dev/null +++ b/components/thread_pool/src/thread_pool.cpp @@ -0,0 +1,150 @@ +#include "thread_pool.hpp" + +#include + +using namespace espp; + +ThreadPool::ThreadPool(const Config &config) + : BaseComponent("ThreadPool", config.log_level), config_(config) { + if (config_.worker_count == 0) { + config_.worker_count = 1; + } + + workers_.reserve(config_.worker_count); + for (std::size_t i = 0; i < config_.worker_count; ++i) { + auto worker_config = config_.worker_task_config; + worker_config.name = config_.worker_task_config.name + "_" + std::to_string(i); + using namespace std::placeholders; + workers_.emplace_back(espp::Task::make_unique({ + .callback = std::bind(&ThreadPool::worker_task_fn, this, _1, _2, _3), + .task_config = worker_config, + .log_level = config_.log_level, + })); + } + + if (config_.auto_start) { + start(); + } +} + +ThreadPool::~ThreadPool() { stop(); } + +void ThreadPool::start() { + if (running_.exchange(true)) { + return; + } + + { + std::lock_guard lock(queue_mutex_); + stopping_ = false; + } + + for (auto &worker : workers_) { + worker->start(); + } +} + +void ThreadPool::stop() { + if (!running_.exchange(false)) { + return; + } + + { + std::lock_guard lock(queue_mutex_); + stopping_ = true; + } + + queue_has_work_cv_.notify_all(); + queue_has_space_cv_.notify_all(); + + for (auto &worker : workers_) { + worker->stop(); + } +} + +bool ThreadPool::is_running() const { return running_.load(); } + +bool ThreadPool::submit(const Job &job) { + return submit_impl(job, config_.block_on_submit_when_full); +} + +bool ThreadPool::try_submit(const Job &job) { return submit_impl(job, false); } + +bool ThreadPool::submit_impl(const Job &job, bool allow_blocking_when_full) { + if (!job) { + rejected_++; + return false; + } + + std::unique_lock lock(queue_mutex_); + if (!running_.load() || stopping_) { + rejected_++; + return false; + } + + if (config_.max_queue_size > 0) { + if (allow_blocking_when_full) { + queue_has_space_cv_.wait(lock, [&]() { + return stopping_ || queue_.size() < config_.max_queue_size; + }); + if (stopping_) { + rejected_++; + return false; + } + } else if (queue_.size() >= config_.max_queue_size) { + rejected_++; + return false; + } + } + + queue_.push_back(job); + submitted_++; + lock.unlock(); + queue_has_work_cv_.notify_one(); + return true; +} + +std::size_t ThreadPool::queue_size() const { + std::lock_guard lock(queue_mutex_); + return queue_.size(); +} + +std::size_t ThreadPool::worker_count() const { return workers_.size(); } + +ThreadPool::Stats ThreadPool::stats() const { + return { + .submitted = submitted_.load(), + .executed = executed_.load(), + .rejected = rejected_.load(), + }; +} + +bool ThreadPool::worker_task_fn(std::mutex &task_mutex, + std::condition_variable &task_cv, + bool &task_notified) { + (void)task_cv; + + Job job; + { + std::unique_lock lock(queue_mutex_); + queue_has_work_cv_.wait(lock, [&]() { return stopping_ || !queue_.empty(); }); + + if (queue_.empty()) { + std::unique_lock task_lock(task_mutex); + return stopping_ || task_notified; + } + + job = std::move(queue_.front()); + queue_.pop_front(); + + if (config_.max_queue_size > 0) { + queue_has_space_cv_.notify_one(); + } + } + + job(); + executed_++; + + std::unique_lock task_lock(task_mutex); + return task_notified; +} diff --git a/pc/CMakeLists.txt b/pc/CMakeLists.txt index 494b59bc2..b30a1cc81 100644 --- a/pc/CMakeLists.txt +++ b/pc/CMakeLists.txt @@ -4,6 +4,29 @@ set(CMAKE_CXX_STANDARD 20) include(${CMAKE_CURRENT_SOURCE_DIR}/../lib/espp.cmake) +set(RTPS_EMBEDDED_COMPONENT_DIR ${CMAKE_CURRENT_SOURCE_DIR}/../components/rtps_embedded) +set(RTPS_EMBEDDED_SOURCES + ${RTPS_EMBEDDED_COMPONENT_DIR}/src/ThreadPool.cpp + ${RTPS_EMBEDDED_COMPONENT_DIR}/src/communication/EsppTransport.cpp + ${RTPS_EMBEDDED_COMPONENT_DIR}/src/discovery/ParticipantProxyData.cpp + ${RTPS_EMBEDDED_COMPONENT_DIR}/src/discovery/SEDPAgent.cpp + ${RTPS_EMBEDDED_COMPONENT_DIR}/src/discovery/SPDPAgent.cpp + ${RTPS_EMBEDDED_COMPONENT_DIR}/src/discovery/TopicData.cpp + ${RTPS_EMBEDDED_COMPONENT_DIR}/src/entities/Domain.cpp + ${RTPS_EMBEDDED_COMPONENT_DIR}/src/entities/Participant.cpp + ${RTPS_EMBEDDED_COMPONENT_DIR}/src/entities/Reader.cpp + ${RTPS_EMBEDDED_COMPONENT_DIR}/src/entities/StatelessReader.cpp + ${RTPS_EMBEDDED_COMPONENT_DIR}/src/entities/Writer.cpp + ${RTPS_EMBEDDED_COMPONENT_DIR}/src/messages/MessageReceiver.cpp + ${RTPS_EMBEDDED_COMPONENT_DIR}/src/messages/MessageTypes.cpp + ${RTPS_EMBEDDED_COMPONENT_DIR}/src/utils/Diagnostics.cpp + ${RTPS_EMBEDDED_COMPONENT_DIR}/thirdparty/Micro-CDR/src/c/common.c + ${RTPS_EMBEDDED_COMPONENT_DIR}/thirdparty/Micro-CDR/src/c/types/array.c + ${RTPS_EMBEDDED_COMPONENT_DIR}/thirdparty/Micro-CDR/src/c/types/basic.c + ${RTPS_EMBEDDED_COMPONENT_DIR}/thirdparty/Micro-CDR/src/c/types/sequence.c + ${RTPS_EMBEDDED_COMPONENT_DIR}/thirdparty/Micro-CDR/src/c/types/string.c +) + MACRO(GEN_TESTS curdir) # get test files FILE(GLOB tests RELATIVE ${curdir} ${curdir}/tests/*.cpp) @@ -11,7 +34,7 @@ MACRO(GEN_TESTS curdir) FOREACH(test_file ${tests}) get_filename_component(TEST_NAME ${test_file} NAME_WE) project(${TEST_NAME} - LANGUAGES CXX + LANGUAGES C CXX VERSION 1.0.0 ) # settings for Windows / MSVC @@ -20,6 +43,17 @@ MACRO(GEN_TESTS curdir) add_definitions(-D_CRT_SECURE_NO_WARNINGS) endif() add_executable(${TEST_NAME} ${test_file}) + + if(TEST_NAME MATCHES "^rtps_embedded_.*") + target_sources(${TEST_NAME} PRIVATE ${RTPS_EMBEDDED_SOURCES}) + target_sources(${TEST_NAME} PRIVATE + ${curdir}/../components/thread_pool/src/thread_pool.cpp) + target_include_directories(${TEST_NAME} PRIVATE + ${curdir}/../components/thread_pool/include + ${RTPS_EMBEDDED_COMPONENT_DIR}/include + ${RTPS_EMBEDDED_COMPONENT_DIR}/thirdparty/Micro-CDR/include) + endif() + target_include_directories(${TEST_NAME} PRIVATE ${curdir}/../lib/pc/include) target_link_directories(${TEST_NAME} PRIVATE diff --git a/pc/tests/rtps_embedded_publisher.cpp b/pc/tests/rtps_embedded_publisher.cpp new file mode 100644 index 000000000..e4a37c593 --- /dev/null +++ b/pc/tests/rtps_embedded_publisher.cpp @@ -0,0 +1,80 @@ +// Standalone embeddedRTPS publisher test for host/PC. It creates an embeddedRTPS Domain, +// participant, and writer, then periodically publishes a uint32 payload. +// +// Usage: rtps_embedded_publisher [topic] [local_ipv4] [period_ms] + +#include +#include +#include +#include + +#include "espp.hpp" +#include "rtps/entities/Domain.h" +#include "rtps_common.hpp" + +namespace { + +bool parse_ipv4(const std::string &ip, rtps::Ip4AddressBytes &out) { + unsigned int b0 = 0, b1 = 0, b2 = 0, b3 = 0; + if (std::sscanf(ip.c_str(), "%u.%u.%u.%u", &b0, &b1, &b2, &b3) != 4) { + return false; + } + if (b0 > 255 || b1 > 255 || b2 > 255 || b3 > 255) { + return false; + } + out = {static_cast(b0), static_cast(b1), static_cast(b2), + static_cast(b3)}; + return true; +} + +} // namespace + +int main(int argc, char **argv) { + espp::Logger logger({.tag = "rtps_embedded_pub", .level = espp::Logger::Verbosity::INFO}); + + const std::string topic = argc > 1 ? argv[1] : "rtps_emb_pub"; + const std::string local_ip_string = argc > 2 ? argv[2] : rtps_test::guess_local_ipv4(); + const int period_ms = argc > 3 ? std::atoi(argv[3]) : 1000; + + rtps::Ip4AddressBytes local_ip{0, 0, 0, 0}; + if (!parse_ipv4(local_ip_string, local_ip)) { + logger.error("Invalid IPv4 address '{}'", local_ip_string); + return 1; + } + + rtps::Domain domain(local_ip); + rtps::Participant *participant = domain.createParticipant(); + if (participant == nullptr) { + logger.error("Failed to create embeddedRTPS participant"); + return 1; + } + + // Keep topic/type names short because embeddedRTPS desktop config caps lengths. + rtps::Writer *writer = + domain.createWriter(*participant, topic.c_str(), "UInt32", false); + if (writer == nullptr) { + logger.error("Failed to create embeddedRTPS writer for topic '{}'", topic); + return 1; + } + + if (!domain.completeInit()) { + logger.error("Failed to start embeddedRTPS domain"); + return 1; + } + + logger.info("publishing on '{}' from {} every {}ms (Ctrl-C to stop)", topic, + local_ip_string, period_ms); + + uint32_t value = 0; + while (true) { + ++value; + const rtps::CacheChange *change = + writer->newChange(rtps::ChangeKind_t::ALIVE, + reinterpret_cast(&value), + static_cast(sizeof(value))); + logger.info("publish {} -> {}", value, change != nullptr ? "queued" : "dropped"); + std::this_thread::sleep_for(std::chrono::milliseconds(period_ms)); + } + + return 0; +} \ No newline at end of file diff --git a/pc/tests/rtps_embedded_subscriber.cpp b/pc/tests/rtps_embedded_subscriber.cpp new file mode 100644 index 000000000..fdb1d75e9 --- /dev/null +++ b/pc/tests/rtps_embedded_subscriber.cpp @@ -0,0 +1,133 @@ +// Standalone embeddedRTPS subscriber test for host/PC. It creates an embeddedRTPS Domain, +// participant, and reader, then logs received std_msgs::msg::String payloads. +// +// Usage: rtps_embedded_subscriber [topic] [local_ipv4] [run_seconds] + +#include +#include +#include +#include +#include +#include + +#include "espp.hpp" +#include "rtps/entities/Domain.h" +#include "rtps_common.hpp" + +namespace { + +struct SubscriberState { + std::atomic received_count{0}; + std::string last_message; + std::mutex mutex; +}; + +bool parse_ipv4(const std::string &ip, rtps::Ip4AddressBytes &out) { + unsigned int b0 = 0, b1 = 0, b2 = 0, b3 = 0; + if (std::sscanf(ip.c_str(), "%u.%u.%u.%u", &b0, &b1, &b2, &b3) != 4) { + return false; + } + if (b0 > 255 || b1 > 255 || b2 > 255 || b3 > 255) { + return false; + } + out = {static_cast(b0), static_cast(b1), static_cast(b2), + static_cast(b3)}; + return true; +} + +void on_sample(void *arg, const rtps::ReaderCacheChange &change) { + if (arg == nullptr) { + return; + } + auto *state = static_cast(arg); + const auto size = change.getDataSize(); + if (size == 0) { + return; + } + + std::string message(size, '\0'); + if (!change.copyInto(reinterpret_cast(message.data()), + static_cast(message.size()))) { + return; + } + + if (!message.empty() && message.back() == '\0') { + message.pop_back(); + } + + { + std::lock_guard lock(state->mutex); + state->last_message = std::move(message); + } + state->received_count++; +} + +} // namespace + +int main(int argc, char **argv) { + espp::Logger logger({.tag = "rtps_embedded_sub", .level = espp::Logger::Verbosity::INFO}); + + const std::string topic = argc > 1 ? argv[1] : "rtps_emb_pub"; + const std::string local_ip_string = argc > 2 ? argv[2] : rtps_test::guess_local_ipv4(); + const int run_seconds = argc > 3 ? std::atoi(argv[3]) : 0; + + rtps::Ip4AddressBytes local_ip{0, 0, 0, 0}; + if (!parse_ipv4(local_ip_string, local_ip)) { + logger.error("Invalid IPv4 address '{}'", local_ip_string); + return 1; + } + + rtps::Domain domain(local_ip); + rtps::Participant *participant = domain.createParticipant(); + if (participant == nullptr) { + logger.error("Failed to create embeddedRTPS participant"); + return 1; + } + + // Keep topic/type names short because embeddedRTPS desktop config caps lengths. + rtps::Reader *reader = + domain.createReader(*participant, topic.c_str(), "std_msgs::msg::String", false); + if (reader == nullptr) { + logger.error("Failed to create embeddedRTPS reader for topic '{}'", topic); + return 1; + } + + SubscriberState state; + if (reader->registerCallback(on_sample, &state) == 0) { + logger.error("Failed to register reader callback"); + return 1; + } + + if (!domain.completeInit()) { + logger.error("Failed to start embeddedRTPS domain"); + return 1; + } + + logger.info("subscribing on '{}' from {} (run_seconds={} / 0=forever)", topic, + local_ip_string, run_seconds); + + const auto start_time = std::chrono::steady_clock::now(); + while (true) { + std::string last_message; + { + std::lock_guard lock(state.mutex); + last_message = state.last_message; + } + logger.info("received {} samples, last='{}'", state.received_count.load(), + last_message); + std::this_thread::sleep_for(std::chrono::seconds(1)); + + if (run_seconds > 0) { + const auto elapsed = + std::chrono::duration_cast(std::chrono::steady_clock::now() - + start_time) + .count(); + if (elapsed >= run_seconds) { + break; + } + } + } + + domain.stop(); + return state.received_count.load() > 0 ? 0 : 1; +} \ No newline at end of file