diff --git a/.gitignore b/.gitignore index de0ec4ac9..f57473991 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..5657e1cc7 --- /dev/null +++ b/components/rtps_embedded/CMakeLists.txt @@ -0,0 +1,28 @@ +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 task thread_pool socket +) + 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..990e8e493 --- /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 esp32-ethernet-kit" + CACHE STRING + "List of components to include" + ) + +project(rtps_embedded_example) + +set(CMAKE_CXX_STANDARD 20) diff --git a/components/rtps_embedded/example/main/CMakeLists.txt b/components/rtps_embedded/example/main/CMakeLists.txt new file mode 100644 index 000000000..68eace2a9 --- /dev/null +++ b/components/rtps_embedded/example/main/CMakeLists.txt @@ -0,0 +1,3 @@ +idf_component_register(SRC_DIRS "." + INCLUDE_DIRS "." + ) diff --git a/components/rtps_embedded/example/main/Kconfig.projbuild b/components/rtps_embedded/example/main/Kconfig.projbuild new file mode 100644 index 000000000..ea5538359 --- /dev/null +++ b/components/rtps_embedded/example/main/Kconfig.projbuild @@ -0,0 +1,10 @@ +menu "RTPS Example Configuration" + + config RTPS_EXAMPLE_ANNOUNCE_PERIOD_MS + int "Publish period (ms)" + range 200 60000 + default 1500 + help + Period between outgoing messages published by the MCU. + +endmenu diff --git a/components/rtps_embedded/example/main/main.cpp b/components/rtps_embedded/example/main/main.cpp new file mode 100644 index 000000000..9a0191f67 --- /dev/null +++ b/components/rtps_embedded/example/main/main.cpp @@ -0,0 +1,187 @@ + +#include +#include +#include + +#include "esp_log.h" +#include "freertos/FreeRTOS.h" +#include "freertos/task.h" + +#include "esp32-ethernet-kit.hpp" +#include "logger.hpp" +#include "rtps/entities/Domain.hpp" +#include "ucdr/microcdr.h" + +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; + } + + // CDR_LE encapsulation header (scheme 0x00 0x01 + 2 zero option bytes) followed by the string. + uint8_t cdr_buf[4 + 4 + 128]; // encap + CDR length prefix + max text (127 chars + null) + cdr_buf[0] = 0x00; + cdr_buf[1] = 0x01; + cdr_buf[2] = 0x00; + cdr_buf[3] = 0x00; + + ucdrBuffer ub; + ucdr_init_buffer_origin_offset_endian(&ub, cdr_buf, sizeof(cdr_buf), 0, 4, + UCDR_LITTLE_ENDIANNESS); + if (!ucdr_serialize_string(&ub, text) || ucdr_buffer_has_error(&ub)) { + return false; + } + + const auto total = static_cast(4 + ucdr_buffer_length(&ub)); + const rtps::CacheChange *change = s_writer->newChange(rtps::ChangeKind_t::ALIVE, cdr_buf, total); + return change != nullptr; +} + +void reader_cb(void * /*callee*/, const rtps::ReaderCacheChange &change) { + const rtps::DataSize_t size = change.getDataSize(); + if (size < 5) { // need at least the 4-byte encapsulation header plus data + return; + } + + uint8_t raw[4 + 4 + 128]; // encap + CDR length prefix + max text (127 chars + null) + const auto copy_size = static_cast(size <= sizeof(raw) ? size : sizeof(raw)); + if (!change.copyInto(raw, copy_size)) { + ESP_LOGI(TAG, "Failed to copy RTPS change data into buffer"); + return; + } + + // Decode the CDR string, skipping the 4-byte CDR encapsulation header. + ucdrBuffer ub; + ucdr_init_buffer_origin_offset_endian(&ub, raw, copy_size, 0, 4, UCDR_LITTLE_ENDIANNESS); + char text[128] = {0}; + if (!ucdr_deserialize_string(&ub, text, sizeof(text)) || ucdr_buffer_has_error(&ub)) { + ESP_LOGI(TAG, "Failed to deserialize RTPS change data"); + return; + } + + ESP_LOGI(TAG, "rx: %s", text); +} + +void publisher_task(void * /*arg*/) { + uint32_t counter = 0; + while (true) { + char msg[32]; + snprintf(msg, sizeof(msg), "msg %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_ANNOUNCE_PERIOD_MS)); + } +} + +} // namespace + +extern "C" void embedded_rtps_start(const rtps::Ip4AddressBytes &local_ip) { + if (s_started) { + return; + } + + constexpr const char *pub_topic = "mcu_to_pc"; + constexpr const char *sub_topic = "pc_to_mcu"; + + 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, "std_msgs::msg::String", true); + if (s_writer == nullptr) { + ESP_LOGE(TAG, "Failed to create RTPS writer"); + return; + } + + s_reader = s_domain->createReader(*s_participant, sub_topic, "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; + } + + xTaskCreate(publisher_task, "rtps_pub", 4096, nullptr, 5, nullptr); + + s_started = true; + ESP_LOGI(TAG, "started: pub=%s sub=%s", pub_topic, sub_topic); +} + +extern "C" void app_main(void) { + espp::Logger logger({.tag = TAG, .level = espp::Logger::Verbosity::INFO}); + + //! [rtps example] + auto &board = espp::Esp32EthernetKit::get(); + + // Static IP 192.168.4.1/24 — ip_info zero-initialised uses that default. + espp::Esp32EthernetKit::ServerConfig srv_cfg; + + bool eth_ok = board.initialize_ethernet({ + .mode = espp::Esp32EthernetKit::DhcpMode::SERVER, + .server_config = srv_cfg, + .on_link_up = [&]() { logger.info("Ethernet link up"); }, + .on_link_down = [&]() { logger.warn("Ethernet link down"); }, + .on_got_ip = + [&](esp_ip4_addr_t ip) { + logger.info("Ethernet DHCP server ready at {}.{}.{}.{}", esp_ip4_addr1_16(&ip), + esp_ip4_addr2_16(&ip), esp_ip4_addr3_16(&ip), esp_ip4_addr4_16(&ip)); + }, + .on_lost_ip = [&]() { logger.warn("Ethernet lost IP"); }, + }); + + if (!eth_ok) { + logger.error("Ethernet initialization failed"); + return; + } + + logger.info("Waiting for Ethernet link..."); + while (!board.is_ethernet_connected()) { + std::this_thread::sleep_for(100ms); + } + + auto eth_ip = board.ethernet_ip(); + logger.info("Ethernet up, IP {}.{}.{}.{}", esp_ip4_addr1_16(ð_ip), esp_ip4_addr2_16(ð_ip), + esp_ip4_addr3_16(ð_ip), esp_ip4_addr4_16(ð_ip)); + + rtps::Ip4AddressBytes local_ip{ + static_cast(esp_ip4_addr1_16(ð_ip)), + static_cast(esp_ip4_addr2_16(ð_ip)), + static_cast(esp_ip4_addr3_16(ð_ip)), + static_cast(esp_ip4_addr4_16(ð_ip)), + }; + + 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/pc/CMakeLists.txt b/components/rtps_embedded/example/pc/CMakeLists.txt new file mode 100644 index 000000000..9e9454193 --- /dev/null +++ b/components/rtps_embedded/example/pc/CMakeLists.txt @@ -0,0 +1,9 @@ +cmake_minimum_required(VERSION 3.20) +project(rtps_embedded_host_pc_example LANGUAGES CXX) + +find_package(fastdds REQUIRED COMPONENTS shared) +find_package(OpenSSL REQUIRED) + +add_executable(host_pubsub host_pubsub.cpp) +target_compile_features(host_pubsub PRIVATE cxx_std_17) +target_link_libraries(host_pubsub PRIVATE fastdds fastcdr OpenSSL::SSL OpenSSL::Crypto) diff --git a/components/rtps_embedded/example/pc/host_pubsub.cpp b/components/rtps_embedded/example/pc/host_pubsub.cpp new file mode 100644 index 000000000..5f7c6d100 --- /dev/null +++ b/components/rtps_embedded/example/pc/host_pubsub.cpp @@ -0,0 +1,249 @@ +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace { + +using namespace eprosima::fastdds::dds; + +static constexpr uint32_t kDomainId = 0; +static constexpr uint16_t kSpdpMulticastPort = 7400; +static constexpr char kTypeName[] = "std_msgs::msg::String"; + +class RawStringPubSubType : public TopicDataType { +public: + RawStringPubSubType() { + set_name(kTypeName); + max_serialized_type_size = 256; + is_compute_key_provided = false; + } + + ~RawStringPubSubType() override = default; + + bool serialize(const void *const data, eprosima::fastdds::rtps::SerializedPayload_t &payload, + DataRepresentationId_t /*rep*/) override { + const std::string *str = static_cast(data); + const uint32_t slen = static_cast(str->size()) + 1u; + const uint32_t needed = 4u + 4u + slen; // encap header + CDR length prefix + string + null + if (needed > payload.max_size) { + return false; + } + // CDR_LE encapsulation header + payload.data[0] = 0x00; + payload.data[1] = 0x01; + payload.data[2] = 0x00; + payload.data[3] = 0x00; + // CDR string: 4-byte LE length followed by string bytes and null terminator + payload.data[4] = static_cast(slen & 0xFF); + payload.data[5] = static_cast((slen >> 8) & 0xFF); + payload.data[6] = static_cast((slen >> 16) & 0xFF); + payload.data[7] = static_cast((slen >> 24) & 0xFF); + std::memcpy(payload.data + 8, str->c_str(), slen); + payload.length = needed; + payload.encapsulation = CDR_LE; + return true; + } + + bool deserialize(eprosima::fastdds::rtps::SerializedPayload_t &payload, void *data) override { + // payload.data includes the 4-byte CDR encapsulation header followed by the CDR string body + if (payload.length < 9u) { + return false; + } + auto *str = static_cast(data); + // Skip encapsulation header, read CDR 4-byte LE string length + const uint32_t slen = static_cast(payload.data[4]) | + (static_cast(payload.data[5]) << 8) | + (static_cast(payload.data[6]) << 16) | + (static_cast(payload.data[7]) << 24); + if (slen == 0 || 8u + slen > payload.length) { + return false; + } + str->assign(reinterpret_cast(payload.data + 8), slen - 1); + return true; + } + + uint32_t calculate_serialized_size(const void *const data, + DataRepresentationId_t /*rep*/) override { + const std::string *str = static_cast(data); + return 4u + 4u + static_cast(str->size()) + 1u; + } + + bool compute_key(eprosima::fastdds::rtps::SerializedPayload_t & /*payload*/, + eprosima::fastdds::rtps::InstanceHandle_t & /*handle*/, + bool /*force_md5*/) override { + return false; + } + + bool compute_key(const void *const /*data*/, + eprosima::fastdds::rtps::InstanceHandle_t & /*handle*/, + bool /*force_md5*/) override { + return false; + } + + void *create_data() override { return new std::string(); } + + void delete_data(void *data) override { delete static_cast(data); } + + void register_type_object_representation() override {} + +#ifdef TOPIC_DATA_TYPE_API_HAS_IS_BOUNDED + bool is_bounded() const override { return false; } +#endif +#ifdef TOPIC_DATA_TYPE_API_HAS_IS_PLAIN + bool is_plain(DataRepresentationId_t) const override { return false; } +#endif +}; + +class StringListener : public DataReaderListener { +public: + explicit StringListener(std::string label) + : label_(std::move(label)) {} + + void on_data_available(DataReader *reader) override { + std::string sample; + SampleInfo info; + while (reader->take_next_sample(&sample, &info) == RETCODE_OK) { + if (info.valid_data) { + std::cout << "[rx " << label_ << "] " << sample << std::endl; + } + } + } + +private: + std::string label_; +}; + +std::atomic g_running{true}; + +void signal_handler(int) { g_running.store(false); } + +} // namespace + +int main(int argc, char **argv) { + std::signal(SIGINT, signal_handler); + std::signal(SIGTERM, signal_handler); + + std::string interface_ip = "192.168.4.2"; + int period_ms = 2000; + + for (int i = 1; i < argc; ++i) { + std::string arg = argv[i]; + if (arg == "--interface-ip" && i + 1 < argc) { + interface_ip = argv[++i]; + } else if (arg == "--period-ms" && i + 1 < argc) { + period_ms = std::stoi(argv[++i]); + } else { + std::cerr << "Usage: " << argv[0] << " [--interface-ip ] [--period-ms ]" << std::endl; + return 1; + } + } + + constexpr const char *pub_topic = "pc_to_mcu"; + constexpr const char *sub_topic = "mcu_to_pc"; + std::cout << "Publish: " << pub_topic << "\nSubscribe: " << sub_topic << std::endl; + + auto *factory = DomainParticipantFactory::get_instance(); + DomainParticipantQos qos; + factory->get_default_participant_qos(qos); + + if (!interface_ip.empty()) { + auto transport = std::make_shared(); + transport->interfaceWhiteList.push_back(interface_ip); + transport->sendBufferSize = 65536; + transport->receiveBufferSize = 65536; + qos.transport().use_builtin_transports = false; + qos.transport().user_transports.push_back(transport); + } + + eprosima::fastdds::rtps::Locator_t peer_mcast; + peer_mcast.kind = LOCATOR_KIND_UDPv4; + eprosima::fastdds::rtps::IPLocator::setIPv4(peer_mcast, "239.255.0.1"); + peer_mcast.port = kSpdpMulticastPort; + qos.wire_protocol().builtin.initialPeersList.push_back(peer_mcast); + + auto *participant = factory->create_participant(kDomainId, qos); + if (participant == nullptr) { + std::cerr << "Failed to create participant" << std::endl; + return 1; + } + + TypeSupport type_support(new RawStringPubSubType()); + if (type_support.register_type(participant) != RETCODE_OK) { + std::cerr << "Failed to register type" << std::endl; + factory->delete_participant(participant); + return 1; + } + + auto *topic_pub = + participant->create_topic(pub_topic, type_support.get_type_name(), TOPIC_QOS_DEFAULT); + auto *topic_sub = + participant->create_topic(sub_topic, type_support.get_type_name(), TOPIC_QOS_DEFAULT); + if (topic_pub == nullptr || topic_sub == nullptr) { + std::cerr << "Failed to create topics" << std::endl; + factory->delete_participant(participant); + return 1; + } + + auto *publisher = participant->create_publisher(PUBLISHER_QOS_DEFAULT); + auto *subscriber = participant->create_subscriber(SUBSCRIBER_QOS_DEFAULT); + if (publisher == nullptr || subscriber == nullptr) { + std::cerr << "Failed to create publisher/subscriber" << std::endl; + participant->delete_topic(topic_pub); + participant->delete_topic(topic_sub); + factory->delete_participant(participant); + return 1; + } + + StringListener listener(sub_topic); + auto *reader = subscriber->create_datareader(topic_sub, DATAREADER_QOS_DEFAULT, &listener); + auto *writer = publisher->create_datawriter(topic_pub, DATAWRITER_QOS_DEFAULT); + if (reader == nullptr || writer == nullptr) { + std::cerr << "Failed to create reader/writer" << std::endl; + participant->delete_subscriber(subscriber); + participant->delete_publisher(publisher); + participant->delete_topic(topic_pub); + participant->delete_topic(topic_sub); + factory->delete_participant(participant); + return 1; + } + + std::this_thread::sleep_for(std::chrono::seconds(2)); + + uint32_t counter = 0; + while (g_running.load()) { + std::string msg = "pc " + std::to_string(counter++); + if (writer->write(&msg) == RETCODE_OK) { + std::cout << "[tx] " << msg << std::endl; + } + std::this_thread::sleep_for(std::chrono::milliseconds(period_ms)); + } + + subscriber->delete_datareader(reader); + publisher->delete_datawriter(writer); + participant->delete_subscriber(subscriber); + participant->delete_publisher(publisher); + participant->delete_topic(topic_pub); + participant->delete_topic(topic_sub); + factory->delete_participant(participant); + return 0; +} diff --git a/components/rtps_embedded/example/sdkconfig.defaults b/components/rtps_embedded/example/sdkconfig.defaults new file mode 100644 index 000000000..bb262760f --- /dev/null +++ b/components/rtps_embedded/example/sdkconfig.defaults @@ -0,0 +1,20 @@ +# Common ESP-related +# +CONFIG_ESP_SYSTEM_EVENT_TASK_STACK_SIZE=4096 +CONFIG_ESP_MAIN_TASK_STACK_SIZE=8192 + +# +# Ethernet (ESP32-Ethernet-Kit A V1.2 — IP101GRI RMII PHY) +# +CONFIG_ETH_ENABLED=y +CONFIG_ETH_USE_ESP32_EMAC=y +CONFIG_ETH_PHY_USE_IP101=y + +# +# Partition Table +# +CONFIG_PARTITION_TABLE_CUSTOM=y +CONFIG_PARTITION_TABLE_CUSTOM_FILENAME="partitions.csv" +CONFIG_PARTITION_TABLE_FILENAME="partitions.csv" +CONFIG_PARTITION_TABLE_OFFSET=0x8000 +CONFIG_PARTITION_TABLE_MD5=y diff --git a/components/rtps_embedded/idf_component.yml b/components/rtps_embedded/idf_component.yml new file mode 100644 index 000000000..19b57f659 --- /dev/null +++ b/components/rtps_embedded/idf_component.yml @@ -0,0 +1,23 @@ +## IDF Component Manager Manifest File +license: "MIT" +description: "embeddedRTPS component for ESP-IDF using espp transport and task abstractions" +url: "https://github.com/esp-cpp/espp/tree/main/components/rtps_embedded" +repository: "git://github.com/esp-cpp/espp.git" +maintainers: + - William Emfinger + - Liyun Guo +examples: + - path: example +tags: + - cpp + - RTPS + - DDS + - networking + - embedded +dependencies: + idf: + version: '>=5.0' + espp/base_component: '>=1.0' + espp/task: '>=1.0' + espp/thread_pool: '>=1.0' + espp/socket: '>=1.0' diff --git a/components/rtps_embedded/include/rtps/ThreadPool.hpp b/components/rtps_embedded/include/rtps/ThreadPool.hpp new file mode 100644 index 000000000..ff05715ce --- /dev/null +++ b/components/rtps_embedded/include/rtps/ThreadPool.hpp @@ -0,0 +1,107 @@ +/* +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/common/types.hpp" +#include "rtps/communication/PacketInfo.hpp" +#include "rtps/config.hpp" +#include "rtps/storages/ThreadSafeCircularBuffer.hpp" + +#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; + using BufferMetatrafficOutgoing = + ThreadSafeCircularBuffer; + using BufferUsertrafficIncoming = + ThreadSafeCircularBuffer; + using BufferMetatrafficIncoming = + ThreadSafeCircularBuffer; + + 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.hpp b/components/rtps_embedded/include/rtps/common/Locator.hpp new file mode 100644 index 000000000..8879b5930 --- /dev/null +++ b/components/rtps_embedded/include/rtps/common/Locator.hpp @@ -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_LOCATOR_T_H +#define RTPS_LOCATOR_T_H + +#include "rtps/common/types.hpp" +#include "rtps/utils/udpUtils.hpp" +#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 { + return ucdr_deserialize_array_uint8_t(&buffer, reinterpret_cast(this), + sizeof(FullLengthLocator)); + } + } + + bool serializeIntoUdcrBuffer(ucdrBuffer &buffer) { + if (ucdr_buffer_remaining(&buffer) < sizeof(FullLengthLocator)) { + return false; + } else { + return 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) const { + return getIp4AddressBytes() == ipAddress; + } + + inline bool isSameSubnet(const std::array &localIp) const { + return isSameSubnetAddress(getIp4AddressBytes(), localIp); + } + + inline bool isMulticastAddress() const { + const auto ip = getIp4AddressBytes(); + return ip[0] >= 224 && ip[0] <= 239; + } + + inline uint32_t getLocatorPort() const { return static_cast(port); } + +} RTPS_EMBEDDED_PACKED; + +inline FullLengthLocator getBuiltInUnicastLocator(ParticipantId_t participantId, + const std::array &localIp) { + return FullLengthLocator::createUDPv4Locator(localIp[0], localIp[1], localIp[2], localIp[3], + getBuiltInUnicastPort(participantId)); +} + +inline FullLengthLocator getBuiltInMulticastLocator() { + return FullLengthLocator::createUDPv4Locator(239, 255, 0, 1, getBuiltInMulticastPort()); +} + +inline FullLengthLocator getUserUnicastLocator(ParticipantId_t participantId, + const std::array &localIp) { + return FullLengthLocator::createUDPv4Locator(localIp[0], localIp[1], localIp[2], localIp[3], + getUserUnicastPort(participantId)); +} + +inline FullLengthLocator +getUserMulticastLocator(const std::array &localIp) { // this would be a unicastaddress, + // as defined in config + return FullLengthLocator::createUDPv4Locator(localIp[0], localIp[1], localIp[2], localIp[3], + getUserMulticastPort()); +} + +inline FullLengthLocator getDefaultSendMulticastLocator() { + return FullLengthLocator::createUDPv4Locator(239, 255, 0, 1, getBuiltInMulticastPort()); +} + +/* + * This representation omits unnecessary 12 bytes of the full RTPS wire format + */ +struct LocatorIPv4 { + LocatorKind_t kind = LocatorKind_t::LOCATOR_KIND_INVALID; + std::array address = {0}; + uint32_t port = LOCATOR_PORT_INVALID; + + LocatorIPv4() = default; + explicit LocatorIPv4(const FullLengthLocator &locator) { + address[0] = locator.address[12]; + address[1] = locator.address[13]; + address[2] = locator.address[14]; + address[3] = locator.address[15]; + port = locator.port; + kind = locator.kind; + } + + std::array getIp4Address() const { return getIp4AddressBytes(); } + + const std::array &getIp4AddressBytes() const { return address; } + + void setInvalid() { kind = LocatorKind_t::LOCATOR_KIND_INVALID; } + + bool isValid() const { return kind != LocatorKind_t::LOCATOR_KIND_INVALID; } + + inline bool isSameSubnet(const std::array &localIp) const { + return isSameSubnetAddress(getIp4AddressBytes(), localIp); + } + + inline bool isMulticastAddress() const { + const auto ip = getIp4AddressBytes(); + return ip[0] >= 224 && ip[0] <= 239; + } +}; + +} // namespace rtps + +#if defined(_MSC_VER) +#pragma pack(pop) +#endif +#undef RTPS_EMBEDDED_PACKED + +#endif // RTPS_LOCATOR_T_H diff --git a/components/rtps_embedded/include/rtps/common/types.hpp b/components/rtps_embedded/include/rtps/common/types.hpp new file mode 100644 index 000000000..68d1be8ca --- /dev/null +++ b/components/rtps_embedded/include/rtps/common/types.hpp @@ -0,0 +1,279 @@ +/* +The MIT License +Copyright (c) 2019 Lehrstuhl Informatik 11 - RWTH Aachen University +Modifications Copyright (c) 2026 ATDev +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: +The above copyright notice and this permission notice shall be included in +all copies or substantial portions of the Software. +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +THE SOFTWARE + +This file is part of embeddedRTPS. + +Author: i11 - Embedded Software, RWTH Aachen University +*/ + +#ifndef RTPS_TYPES_H +#define RTPS_TYPES_H + +#include +#include +#include +#include +#include +#include + +// TODO subnamespaces +namespace rtps { + +// TODO move types to where they are needed! + +typedef uint16_t Ip4Port_t; +typedef uint16_t DataSize_t; +typedef int8_t ParticipantId_t; // With UDP only 120 possible + +enum class EntityKind_t : uint8_t { + USER_DEFINED_UNKNOWN = 0x00, + // No user define participant + USER_DEFINED_WRITER_WITH_KEY = 0x02, + USER_DEFINED_WRITER_WITHOUT_KEY = 0x03, + USER_DEFINED_READER_WITHOUT_KEY = 0x04, + USER_DEFINED_READER_WITH_KEY = 0x07, + + BUILD_IN_UNKNOWN = 0xc0, + BUILD_IN_PARTICIPANT = 0xc1, + BUILD_IN_WRITER_WITH_KEY = 0xc2, + BUILD_IN_WRITER_WITHOUT_KEY = 0xc3, + BUILD_IN_READER_WITHOUT_KEY = 0xc4, + BUILD_IN_READER_WITH_KEY = 0xc7, + + VENDOR_SPEC_UNKNOWN = 0x40, + VENDOR_SPEC_PARTICIPANT = 0x41, + VENDOR_SPEC_WRITER_WITH_KEY = 0x42, + VENDOR_SPEC_WRITER_WITHOUT_KEY = 0x43, + VENDOR_SPEC_READER_WITHOUT_KEY = 0x44, + VENDOR_SPEC_READER_WITH_KEY = 0x47 +}; + +enum class TopicKind_t : uint8_t { NO_KEY = 1, WITH_KEY = 2 }; + +enum class ChangeKind_t : uint8_t { INVALID, ALIVE, NOT_ALIVE_DISPOSED, NOT_ALIVE_UNREGISTERED }; + +enum class ReliabilityKind_t : uint32_t { + BEST_EFFORT = 1, + RELIABLE = 2 // Specification says 3 but eprosima sends 2 +}; + +enum class DurabilityKind_t : uint32_t { + VOLATILE = 0, + TRANSIENT_LOCAL = 1, + TRANSIENT = 2, + PERSISTENT = 3 +}; + +struct GuidPrefix_t { + std::array id; + + bool operator==(const GuidPrefix_t &other) const { return this->id == other.id; } +}; + +struct EntityId_t { + std::array entityKey; + EntityKind_t entityKind; + + bool operator==(const EntityId_t &other) const { + return this->entityKey == other.entityKey && this->entityKind == other.entityKind; + } + + bool operator!=(const EntityId_t &other) const { return !(*this == other); } +}; + +struct Guid_t { + GuidPrefix_t prefix; + EntityId_t entityId; + + bool operator==(const Guid_t &other) const { + return this->prefix == other.prefix && this->entityId == other.entityId; + } + + static uint32_t sum(const Guid_t &other) { + uint32_t ret = std::accumulate(other.prefix.id.begin(), other.prefix.id.end(), uint32_t{0}); + ret = std::accumulate(other.entityId.entityKey.begin(), other.entityId.entityKey.end(), ret); + return ret; + } +}; + +// Described as long but there wasn't any definition. Other than 32 bit does not +// conform the default values +struct Time_t { + int32_t seconds; // time in seconds + uint32_t fraction; // time in sec/2^32 (?) + + static Time_t create(int32_t s, uint32_t ns) { + static constexpr double factor = (static_cast(1) << 32) / 1000000000.; + auto fraction = static_cast(ns * factor); + return Time_t{s, fraction}; + } +}; + +struct VendorId_t { + std::array vendorId; +}; + +struct SequenceNumber_t { + int32_t high; + uint32_t low; + + bool operator==(const SequenceNumber_t &other) const { + return high == other.high && low == other.low; + } + + bool operator!=(const SequenceNumber_t &other) const { return !(*this == other); } + + bool operator<(const SequenceNumber_t &other) const { + return high < other.high || (high == other.high && low < other.low); + } + + bool operator>(const SequenceNumber_t &other) const { + return high > other.high || (high == other.high && low > other.low); + } + + bool operator<=(const SequenceNumber_t &other) const { return *this == other || *this < other; } + + SequenceNumber_t &operator++() { + ++low; + if (low == 0) { + ++high; + } + return *this; + } + + SequenceNumber_t &operator--() { + if (low == 0) { + --high; + low = std::numeric_limits::max(); + } else { + --low; + } + + return *this; + } + + SequenceNumber_t operator++(int) { + SequenceNumber_t tmp(*this); + ++*this; + return tmp; + } +}; + +#define SNS_MAX_NUM_BITS 256 +#define SNS_NUM_BYTES (SNS_MAX_NUM_BITS / 8) +static_assert(!(SNS_MAX_NUM_BITS % 32) && SNS_MAX_NUM_BITS != 0, + "SNS_MAX_NUM_BITS must be multiple of 32"); + +struct SequenceNumberSet { + + SequenceNumberSet() = default; + explicit SequenceNumberSet(const SequenceNumber_t &firstMissing) + : base(firstMissing) {} + + SequenceNumber_t base = {0, 0}; + // Cannot be static because of packed + uint32_t numBits = 0; + std::array bitMap{}; + + // We only need 1 byte because atm we don't store packets. + bool isSet(uint32_t bit) const { + if (bit >= SNS_MAX_NUM_BITS) { + return true; + } + const auto bucket = static_cast(bit / 32); + const auto pos = static_cast(bit % 32); + return (bitMap[bucket] & (1 << (31 - pos))) != 0; + } +}; + +struct FragmentNumber_t { + uint32_t value; +}; + +struct Count_t { + int32_t value; +}; + +struct ProtocolVersion_t { + uint8_t major; + uint8_t minor; +}; + +typedef Time_t Duration_t; // TODO + +enum class ChangeForReaderStatusKind { UNSENT, UNACKNOWLEDGED, REQURESTED, ACKNOWLEDGED, UNDERWAY }; + +enum class ChangeFromWriterStatusKind { LOST, MISSING, RECEIVED, UNKNOWN }; + +struct InstanceHandle_t { // TODO + uint64_t value; +}; + +struct ParticipantMessageData { // TODO +}; + +using Ip4AddressBytes = std::array; + +using ReceiveCallback = void (*)(void *arg, const uint8_t *data, std::size_t size, + Ip4Port_t localPort, Ip4Port_t remotePort, + const Ip4AddressBytes &remoteAddress); + +/* Default Values */ +const EntityId_t ENTITYID_UNKNOWN{}; +const EntityId_t ENTITYID_BUILD_IN_PARTICIPANT = {{00, 00, 01}, EntityKind_t::BUILD_IN_PARTICIPANT}; +const EntityId_t ENTITYID_SEDP_BUILTIN_TOPIC_WRITER = {{00, 00, 02}, + EntityKind_t::BUILD_IN_WRITER_WITH_KEY}; +const EntityId_t ENTITYID_SEDP_BUILTIN_TOPIC_READER = {{00, 00, 02}, + EntityKind_t::BUILD_IN_READER_WITH_KEY}; +const EntityId_t ENTITYID_SEDP_BUILTIN_PUBLICATIONS_WRITER = { + {00, 00, 03}, EntityKind_t::BUILD_IN_WRITER_WITH_KEY}; +const EntityId_t ENTITYID_SEDP_BUILTIN_PUBLICATIONS_READER = { + {00, 00, 03}, EntityKind_t::BUILD_IN_READER_WITH_KEY}; +const EntityId_t ENTITYID_SEDP_BUILTIN_SUBSCRIPTIONS_WRITER = { + {00, 00, 04}, EntityKind_t::BUILD_IN_WRITER_WITH_KEY}; +const EntityId_t ENTITYID_SEDP_BUILTIN_SUBSCRIPTIONS_READER = { + {00, 00, 04}, EntityKind_t::BUILD_IN_READER_WITH_KEY}; +const EntityId_t ENTITYID_SPDP_BUILTIN_PARTICIPANT_WRITER = { + {00, 01, 00}, EntityKind_t::BUILD_IN_WRITER_WITH_KEY}; +const EntityId_t ENTITYID_SPDP_BUILTIN_PARTICIPANT_READER = { + {00, 01, 00}, EntityKind_t::BUILD_IN_READER_WITH_KEY}; +const EntityId_t ENTITYID_P2P_BUILTIN_PARTICIPANT_MESSAGE_WRITER = { + {00, 02, 00}, EntityKind_t::BUILD_IN_WRITER_WITH_KEY}; +const EntityId_t ENTITYID_P2P_BUILTIN_PARTICIPANT_MESSAGE_READER = { + {00, 02, 00}, EntityKind_t::BUILD_IN_READER_WITH_KEY}; + +const GuidPrefix_t GUIDPREFIX_UNKNOWN{}; +const Guid_t GUID_UNKNOWN{}; +const GuidPrefix_t GUID_RANDOM{9, 9, 9, 9, 9, 9, 9, 9, 9, 9, 9}; + +const ParticipantId_t PARTICIPANT_ID_INVALID = -1; + +const ProtocolVersion_t PROTOCOLVERSION = {2, 2}; + +const SequenceNumber_t SEQUENCENUMBER_UNKNOWN = {-1, 0}; + +const Time_t TIME_ZERO = {}; +const Time_t TIME_INVALID = {-1, 0xFFFFFFFF}; +const Time_t TIME_INFINITY = {0x7FFFFFFF, 0xFFFFFFFF}; + +const VendorId_t VENDOR_UNKNOWN = {}; +} // namespace rtps + +#endif // RTPS_TYPES_H diff --git a/components/rtps_embedded/include/rtps/communication/EsppTransport.hpp b/components/rtps_embedded/include/rtps/communication/EsppTransport.hpp new file mode 100644 index 000000000..f914d5005 --- /dev/null +++ b/components/rtps_embedded/include/rtps/communication/EsppTransport.hpp @@ -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.hpp" +#include "rtps/communication/PacketInfo.hpp" +#include "rtps/config.hpp" +#include "udp_socket.hpp" + +#include +#include +#include +#include +#include + +namespace rtps { + +class EsppTransport : public espp::BaseComponent { +public: + using RxCallback = ReceiveCallback; + + EsppTransport(RxCallback callback, void *args); + ~EsppTransport() = default; + + 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.hpp b/components/rtps_embedded/include/rtps/communication/PacketInfo.hpp new file mode 100644 index 000000000..1ed894742 --- /dev/null +++ b/components/rtps_embedded/include/rtps/communication/PacketInfo.hpp @@ -0,0 +1,64 @@ +/* +The MIT License +Copyright (c) 2019 Lehrstuhl Informatik 11 - RWTH Aachen University +Modifications Copyright (c) 2026 ATDev +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: +The above copyright notice and this permission notice shall be included in +all copies or substantial portions of the Software. +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +THE SOFTWARE + +This file is part of embeddedRTPS. + +Author: i11 - Embedded Software, RWTH Aachen University +*/ + +// Copyright 2023 Apex.AI, Inc. +// All rights reserved. + +#ifndef RTPS_PACKETINFO_H +#define RTPS_PACKETINFO_H + +#include +#include + +#include "rtps/common/types.hpp" + +namespace rtps { + +struct PacketInfo { + Ip4Port_t srcPort; // TODO Do we need that? + std::array destAddr = {0, 0, 0, 0}; + Ip4Port_t destPort; + std::vector payload; + + void copyTriviallyCopyable(const PacketInfo &other) { + this->srcPort = other.srcPort; + this->destPort = other.destPort; + this->destAddr = other.destAddr; + } + + PacketInfo() = default; + ~PacketInfo() = default; + + PacketInfo &operator=(const PacketInfo &other) = delete; + + PacketInfo &operator=(PacketInfo &&other) noexcept { + copyTriviallyCopyable(other); + this->payload = std::move(other.payload); + return *this; + } +}; +} // namespace rtps + +#endif // RTPS_PACKETINFO_H diff --git a/components/rtps_embedded/include/rtps/config.hpp b/components/rtps_embedded/include/rtps/config.hpp new file mode 100644 index 000000000..43abe3790 --- /dev/null +++ b/components/rtps_embedded/include/rtps/config.hpp @@ -0,0 +1,39 @@ +/* +The MIT License +Copyright (c) 2019 Lehrstuhl Informatik 11 - RWTH Aachen University +Modifications Copyright (c) 2026 ATDev +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: +The above copyright notice and this permission notice shall be included in +all copies or substantial portions of the Software. +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +THE SOFTWARE + +This file is part of embeddedRTPS. + +Author: i11 - Embedded Software, RWTH Aachen University +*/ + +#ifndef RTPS_CONFIG_H +#define RTPS_CONFIG_H + +#ifdef RTPS_CONFIG_HEADER +#include RTPS_CONFIG_HEADER +#else +#if defined(ESP_PLATFORM) +#include "rtps/config_esp32.hpp" +#else +#include "rtps/config_desktop.hpp" +#endif +#endif + +#endif // RTPS_CONFIG_H diff --git a/components/rtps_embedded/include/rtps/config_desktop.hpp b/components/rtps_embedded/include/rtps/config_desktop.hpp new file mode 100644 index 000000000..be4de0554 --- /dev/null +++ b/components/rtps_embedded/include/rtps/config_desktop.hpp @@ -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_CONFIG_DESKTOP_H +#define RTPS_CONFIG_DESKTOP_H + +#include "rtps/common/types.hpp" + +namespace rtps { + +#define IS_LITTLE_ENDIAN 1 + +namespace Config { +const VendorId_t VENDOR_ID = {13, 37}; +const std::array IP_ADDRESS = {192, 168, 4, 1}; // Needs to be set in lwipcfg.h too. +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.hpp b/components/rtps_embedded/include/rtps/config_esp32.hpp new file mode 100644 index 000000000..53bfc6571 --- /dev/null +++ b/components/rtps_embedded/include/rtps/config_esp32.hpp @@ -0,0 +1,99 @@ +/* +The MIT License +Copyright (c) 2019 Lehrstuhl Informatik 11 - RWTH Aachen University +Modifications Copyright (c) 2026 ATDev +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: +The above copyright notice and this permission notice shall be included in +all copies or substantial portions of the Software. +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +THE SOFTWARE + +This file is part of embeddedRTPS. + +Author: i11 - Embedded Software, RWTH Aachen University +*/ + +#ifndef RTPS_CONFIG_ESP32_H +#define RTPS_CONFIG_ESP32_H + +#include "rtps/common/types.hpp" + +namespace rtps { + +#define IS_LITTLE_ENDIAN 1 +#define OS_IS_FREERTOS + +namespace Config { +const VendorId_t VENDOR_ID = {13, 37}; +const std::array IP_ADDRESS = {192, 168, 4, + 1}; // Fallback: must match DHCPS server netif IP. +const GuidPrefix_t BASE_GUID_PREFIX{1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 13}; + +const uint8_t DOMAIN_ID = 0; // 230 possible with UDP +const uint8_t NUM_STATELESS_WRITERS = 5; +const uint8_t NUM_STATELESS_READERS = 5; +const uint8_t NUM_STATEFUL_READERS = 5; +const uint8_t NUM_STATEFUL_WRITERS = 5; +const uint8_t MAX_NUM_PARTICIPANTS = 1; +const uint8_t NUM_WRITERS_PER_PARTICIPANT = 10; +const uint8_t NUM_READERS_PER_PARTICIPANT = 10; +const uint8_t NUM_WRITER_PROXIES_PER_READER = 6; +const uint8_t NUM_READER_PROXIES_PER_WRITER = 6; + +const uint8_t MAX_NUM_UNMATCHED_REMOTE_WRITERS = 50; +const uint8_t MAX_NUM_UNMATCHED_REMOTE_READERS = 50; + +const uint8_t MAX_NUM_READER_CALLBACKS = 5; + +const uint8_t HISTORY_SIZE_STATELESS = 2; +const uint8_t HISTORY_SIZE_STATEFUL = 10; + +const uint8_t MAX_TYPENAME_LENGTH = 64; +const uint8_t MAX_TOPICNAME_LENGTH = 64; + +const int HEARTBEAT_STACKSIZE = 1024 * 6; // byte +const int THREAD_POOL_WRITER_STACKSIZE = 4096; // byte +const int THREAD_POOL_READER_STACKSIZE = 1024 * 6; // byte +const uint16_t SPDP_WRITER_STACKSIZE = 4096; // byte + +const uint16_t SF_WRITER_HB_PERIOD_MS = 4000; +const uint16_t SPDP_RESEND_PERIOD_MS = 2000; +const uint8_t SPDP_CYCLECOUNT_HEARTBEAT = 2; // skip x SPDP rounds before checking liveliness +const uint8_t SPDP_WRITER_PRIO = 5; +const uint8_t SPDP_MAX_NUMBER_FOUND_PARTICIPANTS = 10; +const uint8_t SPDP_MAX_NUM_LOCATORS = 1; +const Duration_t SPDP_DEFAULT_REMOTE_LEASE_DURATION = { + 5, 0}; // Default lease duration for remote participants, usually + // overwritten by remote info +const Duration_t SPDP_MAX_REMOTE_LEASE_DURATION = { + 90, 0}; // Absolute maximum lease duration, ignoring remote participant info + +const Duration_t SPDP_LEASE_DURATION = {5, 0}; + +const int MAX_NUM_UDP_CONNECTIONS = 10; + +const int THREAD_POOL_NUM_WRITERS = 2; +const int THREAD_POOL_NUM_READERS = 2; +const int THREAD_POOL_WRITER_PRIO = 5; +const int THREAD_POOL_READER_PRIO = 5; +const int THREAD_POOL_WORKLOAD_QUEUE_LENGTH_USERTRAFFIC = 60; +const int THREAD_POOL_WORKLOAD_QUEUE_LENGTH_METATRAFFIC = 60; + +constexpr int OVERALL_HEAP_SIZE = THREAD_POOL_NUM_WRITERS * THREAD_POOL_WRITER_STACKSIZE + + THREAD_POOL_NUM_READERS * THREAD_POOL_READER_STACKSIZE + + MAX_NUM_PARTICIPANTS * SPDP_WRITER_STACKSIZE + + NUM_STATEFUL_WRITERS * HEARTBEAT_STACKSIZE; +} // namespace Config +} // namespace rtps + +#endif // RTPS_CONFIG_ESP32_H diff --git a/components/rtps_embedded/include/rtps/discovery/BuiltInEndpoints.hpp b/components/rtps_embedded/include/rtps/discovery/BuiltInEndpoints.hpp new file mode 100644 index 000000000..dda5ada33 --- /dev/null +++ b/components/rtps_embedded/include/rtps/discovery/BuiltInEndpoints.hpp @@ -0,0 +1,46 @@ +/* +The MIT License +Copyright (c) 2019 Lehrstuhl Informatik 11 - RWTH Aachen University +Modifications Copyright (c) 2026 ATDev +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: +The above copyright notice and this permission notice shall be included in +all copies or substantial portions of the Software. +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +THE SOFTWARE + +This file is part of embeddedRTPS. + +Author: i11 - Embedded Software, RWTH Aachen University +*/ + +#ifndef RTPS_BUILTINENDPOINTS_H +#define RTPS_BUILTINENDPOINTS_H + +#include "rtps/entities/StatefulReader.hpp" +#include "rtps/entities/StatelessReader.hpp" +#include "rtps/entities/StatelessWriter.hpp" +#include "rtps/entities/Writer.hpp" + +namespace rtps { + +struct BuiltInEndpoints { + Writer *spdpWriter = nullptr; + Reader *spdpReader = nullptr; + Writer *sedpPubWriter = nullptr; + Reader *sedpPubReader = nullptr; + Writer *sedpSubWriter = nullptr; + Reader *sedpSubReader = nullptr; +}; +} // namespace rtps + +#endif // RTPS_BUILTINENDPOINTS_H diff --git a/components/rtps_embedded/include/rtps/discovery/ParticipantProxyData.hpp b/components/rtps_embedded/include/rtps/discovery/ParticipantProxyData.hpp new file mode 100644 index 000000000..188675968 --- /dev/null +++ b/components/rtps_embedded/include/rtps/discovery/ParticipantProxyData.hpp @@ -0,0 +1,157 @@ +/* +The MIT License +Copyright (c) 2019 Lehrstuhl Informatik 11 - RWTH Aachen University +Modifications Copyright (c) 2026 ATDev +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: +The above copyright notice and this permission notice shall be included in +all copies or substantial portions of the Software. +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +THE SOFTWARE + +This file is part of embeddedRTPS. + +Author: i11 - Embedded Software, RWTH Aachen University +*/ + +#ifndef RTPS_PARTICIPANTPROXYDATA_H +#define RTPS_PARTICIPANTPROXYDATA_H + +#include "base_component.hpp" +#include "rtps/config.hpp" +#include "rtps/messages/MessageTypes.hpp" +#include "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(); + } + explicit ParticipantProxyData(Guid_t guid); + + ProtocolVersion_t m_protocolVersion = PROTOCOLVERSION; + Guid_t m_guid = Guid_t{GUIDPREFIX_UNKNOWN, ENTITYID_UNKNOWN}; + VendorId_t m_vendorId = VENDOR_UNKNOWN; + bool m_expectsInlineQos = false; + BuiltinEndpointSet_t m_availableBuiltInEndpoints{0}; + std::array m_metatrafficUnicastLocatorList; + std::array m_metatrafficMulticastLocatorList; + std::array m_defaultUnicastLocatorList; + std::array m_defaultMulticastLocatorList; + Count_t m_manualLivelinessCount{1}; + Duration_t m_leaseDuration = Config::SPDP_DEFAULT_REMOTE_LEASE_DURATION; + std::chrono::time_point m_lastLivelinessReceivedTimestamp; + void reset(); + + 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 { + const double factor = static_cast(1000) / + static_cast(1ULL << 32); // Convert fraction to milliseconds + uint32_t lease_in_ms = + m_leaseDuration.seconds * 1000 + static_cast(m_leaseDuration.fraction * factor); + + uint32_t max_lease_in_ms = + Config::SPDP_MAX_REMOTE_LEASE_DURATION.seconds * 1000 + + static_cast(Config::SPDP_MAX_REMOTE_LEASE_DURATION.fraction * factor); + + auto heatbeat_age_in_ms = getAliveSignalAgeInMilliseconds(); + + if (heatbeat_age_in_ms > std::min(lease_in_ms, max_lease_in_ms)) { + return false; + } + return true; +} + +} // namespace rtps +#endif // RTPS_PARTICIPANTPROXYDATA_H diff --git a/components/rtps_embedded/include/rtps/discovery/SEDPAgent.hpp b/components/rtps_embedded/include/rtps/discovery/SEDPAgent.hpp new file mode 100644 index 000000000..ae60cec56 --- /dev/null +++ b/components/rtps_embedded/include/rtps/discovery/SEDPAgent.hpp @@ -0,0 +1,117 @@ +/* +The MIT License +Copyright (c) 2019 Lehrstuhl Informatik 11 - RWTH Aachen University +Modifications Copyright (c) 2026 ATDev +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: +The above copyright notice and this permission notice shall be included in +all copies or substantial portions of the Software. +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +THE SOFTWARE + +This file is part of embeddedRTPS. + +Author: i11 - Embedded Software, RWTH Aachen University +*/ + +#ifndef RTPS_SEDPAGENT_H +#define RTPS_SEDPAGENT_H + +#include "base_component.hpp" +#include "rtps/config.hpp" +#include "rtps/discovery/BuiltInEndpoints.hpp" +#include "rtps/discovery/TopicData.hpp" + +#include +#include + +namespace rtps { + +class Participant; +class ReaderCacheChange; +class Writer; +class Reader; + +class SEDPAgent : public espp::BaseComponent { +public: + SEDPAgent(); + void init(Participant &part, const BuiltInEndpoints &endpoints); + bool addWriter(Writer &writer); + bool addReader(Reader &reader); + bool deleteReader(Reader *reader); + bool deleteWriter(Writer *reader); + + void registerOnNewPublisherMatchedCallback(void (*callback)(void *arg), void *args); + void registerOnNewSubscriberMatchedCallback(void (*callback)(void *arg), void *args); + void removeUnmatchedEntitiesOfParticipant(const GuidPrefix_t &guidPrefix); + void removeUnmatchedEntity(const Guid_t &guid); + + uint32_t getNumRemoteUnmatchedReaders(); + uint32_t getNumRemoteUnmatchedWriters(); + +protected: // For testing purposes + void handlePublisherReaderMessage(const TopicData &writerData, const ReaderCacheChange &change); + void handleSubscriptionReaderMessage(const TopicData &writerData, + const ReaderCacheChange &change); + +private: + Participant *m_part = nullptr; + std::recursive_mutex m_mutex; + uint8_t m_buffer[600]; // TODO check size, currently changed from 300 to 600 + // (FastDDS gives too many options) + BuiltInEndpoints m_endpoints; + /* + * If we add readers later on, remote participants will not send matching + * writer proxies again (and vice versa). This is done only once during + * discovery. Therefore, we need to keep track of remote endpoints. Topic and + * type are represented as hash values to save memory. + */ + MemoryPool + m_unmatchedRemoteWriters; + size_t m_numUnmatchedRemoteWriters = 0; + MemoryPool + m_unmatchedRemoteReaders; + size_t m_numMatchedRemoteReaders = 0; + + void tryMatchUnmatchedEndpoints(); + void addUnmatchedRemoteWriter(const TopicData &writerData); + void addUnmatchedRemoteReader(const TopicData &readerData); + void addUnmatchedRemoteWriter(const TopicDataCompressed &writerData); + void addUnmatchedRemoteReader(const TopicDataCompressed &readerData); + + void handleRemoteEndpointDeletion(const TopicData &topic, const ReaderCacheChange &change); + + void (*mfp_onNewPublisherCallback)(void *arg) = nullptr; + void *m_onNewPublisherArgs = nullptr; + void (*mfp_onNewSubscriberCallback)(void *arg) = nullptr; + void *m_onNewSubscriberArgs = nullptr; + + static void jumppadPublisherReader(void *callee, const ReaderCacheChange &cacheChange); + static void jumppadSubscriptionReader(void *callee, const ReaderCacheChange &cacheChange); + + static void jumppadTakeProxyOfDisposedReader(const Reader *reader, const WriterProxy &proxy, + void *arg); + static void jumppadTakeProxyOfDisposedWriter(const Writer *writer, const ReaderProxy &proxy, + void *arg); + + void handlePublisherReaderMessage(const ReaderCacheChange &change); + void handleSubscriptionReaderMessage(const ReaderCacheChange &change); + + template bool deleteEndpoint(A *endpoint, Writer *sedp_endpoint); + + template bool announceEndpointDeletion(A *local_endpoint, Writer *sedp_endpoint); + + template bool disposeEndpointInSEDPHistory(A *local_endpoint, Writer *sedp_writer); +}; +} // namespace rtps + +#endif // RTPS_SEDPAGENT_H diff --git a/components/rtps_embedded/include/rtps/discovery/SPDPAgent.hpp b/components/rtps_embedded/include/rtps/discovery/SPDPAgent.hpp new file mode 100644 index 000000000..f9a10e8ea --- /dev/null +++ b/components/rtps_embedded/include/rtps/discovery/SPDPAgent.hpp @@ -0,0 +1,90 @@ +/* +The MIT License +Copyright (c) 2019 Lehrstuhl Informatik 11 - RWTH Aachen University +Modifications Copyright (c) 2026 ATDev +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: +The above copyright notice and this permission notice shall be included in +all copies or substantial portions of the Software. +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +THE SOFTWARE + +This file is part of embeddedRTPS. + +Author: i11 - Embedded Software, RWTH Aachen University +*/ + +#ifndef RTPS_SPDP_H +#define RTPS_SPDP_H + +#include "base_component.hpp" +#include "rtps/common/types.hpp" +#include "rtps/config.hpp" +#include "rtps/discovery/BuiltInEndpoints.hpp" +#include "rtps/discovery/ParticipantProxyData.hpp" +#include "rtps/utils/Log.hpp" +#include "task.hpp" +#include "ucdr/microcdr.h" + +#include +#include + +#if SPDP_VERBOSE && RTPS_GLOBAL_VERBOSE +#include "rtps/utils/printutils.hpp" +#define SPDP_LOG(...) logger_.warn(__VA_ARGS__) +#else +#define SPDP_LOG(...) \ + do { \ + } while (0) +#endif + +namespace rtps { +class Participant; +class Writer; +class Reader; +class ReaderCacheChange; + +class SPDPAgent : public espp::BaseComponent { +public: + SPDPAgent(); + void init(Participant &participant, BuiltInEndpoints &endpoints); + void start(); + void stop(); + 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.hpp b/components/rtps_embedded/include/rtps/discovery/TopicData.hpp new file mode 100644 index 000000000..63ccbd4fd --- /dev/null +++ b/components/rtps_embedded/include/rtps/discovery/TopicData.hpp @@ -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_DISCOVEREDWRITERDATA_H +#define RTPS_DISCOVEREDWRITERDATA_H + +#define SUPPRESS_UNICAST 0 + +#include "rtps/config.hpp" +#include "rtps/utils/hash.hpp" +#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 = 0; + bool statusInfoValid = false; + // Use Case: Remotes communicates id of deleted endpoint through key_hash + // parameter + EntityId_t entityIdFromKeyHash = ENTITYID_UNKNOWN; + bool entityIdFromKeyHashValid = false; + + TopicData() + : endpointGuid(GUID_UNKNOWN) + , typeName{'\0'} + , topicName{'\0'} + , reliabilityKind(ReliabilityKind_t::BEST_EFFORT) + , durabilityKind(DurabilityKind_t::VOLATILE) { + rtps::FullLengthLocator someLocator = + rtps::FullLengthLocator::createUDPv4Locator(192, 168, 0, 42, rtps::getUserUnicastPort(0)); + unicastLocator = someLocator; + multicastLocator = FullLengthLocator(); + }; + + TopicData(Guid_t guid, ReliabilityKind_t reliability, FullLengthLocator loc) + : endpointGuid(guid) + , typeName{'\0'} + , topicName{'\0'} + , reliabilityKind(reliability) + , durabilityKind(DurabilityKind_t::VOLATILE) + , unicastLocator(loc) {} + + bool matchesTopicOf(const TopicData &other); + + bool readFromUcdrBuffer(ucdrBuffer &buffer); + bool serializeIntoUcdrBuffer(ucdrBuffer &buffer) const; + + bool isDisposedFlagSet() const; + bool isUnregisteredFlagSet() const; +}; + +struct TopicDataCompressed { + Guid_t endpointGuid = GUID_UNKNOWN; + std::size_t topicHash = 0; + std::size_t typeHash = 0; + bool is_reliable = false; + LocatorIPv4 unicastLocator; + LocatorIPv4 multicastLocator; + + TopicDataCompressed() = default; + explicit TopicDataCompressed(const TopicData &topic_data) + : endpointGuid(topic_data.endpointGuid) + , topicHash(hashCharArray(topic_data.topicName, Config::MAX_TOPICNAME_LENGTH)) + , typeHash(hashCharArray(topic_data.typeName, Config::MAX_TYPENAME_LENGTH)) + , is_reliable(topic_data.reliabilityKind == ReliabilityKind_t::RELIABLE) + , unicastLocator(topic_data.unicastLocator) + , multicastLocator(topic_data.multicastLocator) {} + + bool matchesTopicOf(const TopicData &topic_data) const; +}; +} // namespace rtps + +#endif // RTPS_DISCOVEREDWRITERDATA_H diff --git a/components/rtps_embedded/include/rtps/entities/Domain.hpp b/components/rtps_embedded/include/rtps/entities/Domain.hpp new file mode 100644 index 000000000..b93011594 --- /dev/null +++ b/components/rtps_embedded/include/rtps/entities/Domain.hpp @@ -0,0 +1,106 @@ +/* +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.hpp" +#include "rtps/common/types.hpp" +#include "rtps/communication/EsppTransport.hpp" +#include "rtps/config.hpp" +#include "rtps/entities/Participant.hpp" +#include "rtps/entities/StatefulReader.hpp" +#include "rtps/entities/StatefulWriter.hpp" +#include "rtps/entities/StatelessReader.hpp" +#include "rtps/entities/StatelessWriter.hpp" +#include +#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; + bool m_transportSetupOk = true; + std::recursive_mutex m_mutex; + + void receiveCallback(const PacketInfo &packet); + GuidPrefix_t generateGuidPrefix(ParticipantId_t id) const; + void createBuiltinWritersAndReaders(Participant &part); + bool initializeTransport(); + void 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.hpp b/components/rtps_embedded/include/rtps/entities/Participant.hpp new file mode 100644 index 000000000..bd5ddef3d --- /dev/null +++ b/components/rtps_embedded/include/rtps/entities/Participant.hpp @@ -0,0 +1,129 @@ +/* +The MIT License +Copyright (c) 2019 Lehrstuhl Informatik 11 - RWTH Aachen University +Modifications Copyright (c) 2026 ATDev +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: +The above copyright notice and this permission notice shall be included in +all copies or substantial portions of the Software. +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +THE SOFTWARE + +This file is part of embeddedRTPS. + +Author: i11 - Embedded Software, RWTH Aachen University +*/ + +#ifndef RTPS_PARTICIPANT_H +#define RTPS_PARTICIPANT_H + +#include "base_component.hpp" +#include "rtps/common/types.hpp" +#include "rtps/config.hpp" +#include "rtps/discovery/SEDPAgent.hpp" +#include "rtps/discovery/SPDPAgent.hpp" +#include "rtps/messages/MessageReceiver.hpp" + +#include +#include + +namespace rtps { + +class Writer; +class Reader; + +class Participant : public espp::BaseComponent { +public: + GuidPrefix_t m_guidPrefix; + ParticipantId_t m_participantId; + Ip4AddressBytes m_localIpAddress{{0, 0, 0, 0}}; + + Participant(); + explicit Participant(const GuidPrefix_t &guidPrefix, ParticipantId_t participantId); + + // Not allowed because the message receiver contains a pointer to the + // participant + Participant(const Participant &) = delete; + Participant(Participant &&) = delete; + Participant &operator=(const Participant &) = delete; + Participant &operator=(Participant &&) = delete; + + ~Participant(); + bool isValid(); + + void reuse(const GuidPrefix_t &guidPrefix, ParticipantId_t participantId); + void reuse(const GuidPrefix_t &guidPrefix, ParticipantId_t participantId, + const Ip4AddressBytes &localIpAddress); + + std::array getNextUserEntityKey(); + + // Actually the only two function that should be used by the user + bool registerOnNewPublisherMatchedCallback(void (*callback)(void *arg), void *args); + bool registerOnNewSubscriberMatchedCallback(void (*callback)(void *arg), void *args); + + //! Not-thread-safe function to add a writer + Writer *addWriter(Writer *writer); + bool isWritersFull(); + bool deleteWriter(Writer *writer); + + //! Not-thread-safe function to add a reader + Reader *addReader(Reader *reader); + bool isReadersFull(); + bool deleteReader(Reader *reader); + + //! (Probably) Thread safe if writers cannot be removed + Writer *getWriter(EntityId_t id); + Writer *getMatchingWriter(const TopicData &topicData); + Writer *getMatchingWriter(const TopicDataCompressed &topicData); + + //! (Probably) Thread safe if readers cannot be removed + Reader *getReader(EntityId_t id); + Reader *getReaderByWriterId(const Guid_t &guid); + Reader *getMatchingReader(const TopicData &topicData); + Reader *getMatchingReader(const TopicDataCompressed &topicData); + + bool addNewRemoteParticipant(const ParticipantProxyData &remotePart); + bool removeRemoteParticipant(const GuidPrefix_t &prefix); + void removeAllProxiesOfParticipant(const GuidPrefix_t &prefix); + void removeProxyFromAllEndpoints(const Guid_t &guid); + + const ParticipantProxyData *findRemoteParticipant(const GuidPrefix_t &prefix); + void refreshRemoteParticipantLiveliness(const GuidPrefix_t &prefix); + uint32_t getRemoteParticipantCount(); + MessageReceiver *getMessageReceiver(); + bool checkAndResetHeartbeats(); + + bool hasReaderWithMulticastLocator(const std::array &address); + + void addBuiltInEndpoints(BuiltInEndpoints &endpoints); + void newMessage(const uint8_t *data, DataSize_t size); + + SPDPAgent &getSPDPAgent(); + void printInfo(); + +private: + friend class SizeInspector; + MessageReceiver m_receiver; + bool m_hasBuilInEndpoints = false; + std::array m_nextUserEntityId{{0, 0, 1}}; + std::array m_writers = {nullptr}; + std::array m_readers = {nullptr}; + + std::recursive_mutex m_mutex; + MemoryPool m_remoteParticipants; + + SPDPAgent m_spdpAgent; + SEDPAgent m_sedpAgent; +}; +} // namespace rtps + +#endif // RTPS_PARTICIPANT_H diff --git a/components/rtps_embedded/include/rtps/entities/Reader.hpp b/components/rtps_embedded/include/rtps/entities/Reader.hpp new file mode 100644 index 000000000..96e1558a8 --- /dev/null +++ b/components/rtps_embedded/include/rtps/entities/Reader.hpp @@ -0,0 +1,146 @@ +/* +The MIT License +Copyright (c) 2019 Lehrstuhl Informatik 11 - RWTH Aachen University +Modifications Copyright (c) 2026 ATDev +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: +The above copyright notice and this permission notice shall be included in +all copies or substantial portions of the Software. +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +THE SOFTWARE + +This file is part of embeddedRTPS. + +Author: i11 - Embedded Software, RWTH Aachen University +*/ + +#ifndef RTPS_READER_H +#define RTPS_READER_H + +#include "base_component.hpp" +#include "rtps/common/types.hpp" +#include "rtps/config.hpp" +#include "rtps/discovery/TopicData.hpp" +#include "rtps/entities/WriterProxy.hpp" +#include "rtps/storages/MemoryPool.hpp" +#include +#include + +namespace rtps { + +struct SubmessageHeartbeat; +struct SubmessageGap; + +class ReaderCacheChange { +private: + const uint8_t *data; + +public: + const ChangeKind_t kind; + const DataSize_t size; + const Guid_t writerGuid; + const SequenceNumber_t sn; + + ReaderCacheChange(ChangeKind_t kind, Guid_t &writerGuid, SequenceNumber_t sn, const uint8_t *data, + DataSize_t size) + : data(data) + , kind(kind) + , size(size) + , writerGuid(writerGuid) + , sn(sn){}; + + ~ReaderCacheChange() = default; // No need to free data. It's not owned by this object + // Not allowed because this class doesn't own the ptr and the user isn't + // allowed to use it outside the Scope of the callback + ReaderCacheChange(const ReaderCacheChange &other) = delete; + ReaderCacheChange(ReaderCacheChange &&other) = delete; + ReaderCacheChange &operator=(const ReaderCacheChange &other) = delete; + ReaderCacheChange &operator=(ReaderCacheChange &&other) = delete; + + bool copyInto(uint8_t *buffer, DataSize_t destSize) const { + if (destSize < size) { + return false; + } else { + memcpy(buffer, data, size); + return true; + } + } + + const uint8_t *getData() const { return data; } + + DataSize_t getDataSize() const { return size; } +}; + +typedef void (*ddsReaderCallback_fp)(void *callee, const ReaderCacheChange &cacheChange); + +class Reader : public espp::BaseComponent { +public: + using callbackFunction_t = void (*)(void *, const ReaderCacheChange &); + using callbackIdentifier_t = uint32_t; + + TopicData m_attributes; + virtual void newChange(const ReaderCacheChange &cacheChange) = 0; + virtual callbackIdentifier_t registerCallback(callbackFunction_t cb, void *arg); + virtual bool removeCallback(callbackIdentifier_t identifier); + uint8_t getNumCallbacks(); + + virtual bool onNewHeartbeat(const SubmessageHeartbeat &msg, const GuidPrefix_t &remotePrefix) = 0; + virtual bool onNewGapMessage(const SubmessageGap &msg, const GuidPrefix_t &remotePrefix) = 0; + virtual bool addNewMatchedWriter(const WriterProxy &newProxy) = 0; + virtual bool removeProxy(const Guid_t &guid); + virtual void removeAllProxiesOfParticipant(const GuidPrefix_t &guidPrefix); + bool isInitialized() const { return m_is_initialized_; } + virtual void reset(); + bool isProxy(const Guid_t &guid); + WriterProxy *getProxy(Guid_t guid); + uint32_t getProxiesCount(); + + void setSEDPSequenceNumber(const SequenceNumber_t &sn); + const SequenceNumber_t &getSEDPSequenceNumber(); + + using dumpProxyCallback = void (*)(const Reader *reader, const WriterProxy &, void *arg); + + int dumpAllProxies(dumpProxyCallback target, void *arg); + + virtual bool sendPreemptiveAckNack(const WriterProxy &writer); + +protected: + void executeCallbacks(const ReaderCacheChange &cacheChange); + bool initMutex(); + + SequenceNumber_t m_sedp_sequence_number; + + bool m_is_initialized_ = false; + Reader(); + virtual ~Reader() = default; + MemoryPool m_proxies; + + callbackIdentifier_t m_callback_identifier = 1; + + uint8_t m_callback_count = 0; + using callbackElement_t = struct { + callbackFunction_t function; + void *arg; + callbackIdentifier_t identifier; + }; + + std::array m_callbacks; + + // Guards manipulation of the proxies array + std::recursive_mutex m_proxies_mutex; + + // Guards manipulation of callback array + std::recursive_mutex m_callback_mutex; +}; +} // namespace rtps + +#endif // RTPS_READER_H diff --git a/components/rtps_embedded/include/rtps/entities/ReaderProxy.hpp b/components/rtps_embedded/include/rtps/entities/ReaderProxy.hpp new file mode 100644 index 000000000..298f018f5 --- /dev/null +++ b/components/rtps_embedded/include/rtps/entities/ReaderProxy.hpp @@ -0,0 +1,68 @@ +/* +The MIT License +Copyright (c) 2019 Lehrstuhl Informatik 11 - RWTH Aachen University +Modifications Copyright (c) 2026 ATDev +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: +The above copyright notice and this permission notice shall be included in +all copies or substantial portions of the Software. +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +THE SOFTWARE + +This file is part of embeddedRTPS. + +Author: i11 - Embedded Software, RWTH Aachen University +*/ + +#ifndef RTPS_READERPROXY_H +#define RTPS_READERPROXY_H + +#include "rtps/common/types.hpp" +#include "rtps/discovery/ParticipantProxyData.hpp" + +namespace rtps { +struct ReaderProxy { + Guid_t remoteReaderGuid; + Count_t ackNackCount = {0}; + LocatorIPv4 remoteLocator; + bool is_reliable = false; + LocatorIPv4 remoteMulticastLocator; + bool useMulticast = false; + bool suppressUnicast = false; + bool unknown_eid = false; + bool finalFlag = false; + SequenceNumber_t lastAckNackSequenceNumber = {0, 1}; + + ReaderProxy() + : remoteReaderGuid({GUIDPREFIX_UNKNOWN, ENTITYID_UNKNOWN}) + , ackNackCount{0} + , remoteLocator(LocatorIPv4()) + , finalFlag(false){}; + ReaderProxy(const Guid_t &guid, const LocatorIPv4 &loc, bool reliable) + : remoteReaderGuid(guid) + , ackNackCount{0} + , remoteLocator(loc) + , is_reliable(reliable) + , finalFlag(false){}; + ReaderProxy(const Guid_t &guid, const LocatorIPv4 &loc, const LocatorIPv4 &mcastloc, + bool reliable) + : remoteReaderGuid(guid) + , ackNackCount{0} + , remoteLocator(loc) + , is_reliable(reliable) + , remoteMulticastLocator(mcastloc) + , finalFlag(false){}; +}; + +} // namespace rtps + +#endif // RTPS_READERPROXY_H diff --git a/components/rtps_embedded/include/rtps/entities/StatefulReader.hpp b/components/rtps_embedded/include/rtps/entities/StatefulReader.hpp new file mode 100644 index 000000000..cef1c9479 --- /dev/null +++ b/components/rtps_embedded/include/rtps/entities/StatefulReader.hpp @@ -0,0 +1,65 @@ +/* +The MIT License +Copyright (c) 2019 Lehrstuhl Informatik 11 - RWTH Aachen University +Modifications Copyright (c) 2026 ATDev +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: +The above copyright notice and this permission notice shall be included in +all copies or substantial portions of the Software. +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +THE SOFTWARE + +This file is part of embeddedRTPS. + +Author: i11 - Embedded Software, RWTH Aachen University +*/ + +#ifndef RTPS_STATEFULREADER_H +#define RTPS_STATEFULREADER_H + +#include "rtps/common/types.hpp" +#include "rtps/communication/PacketInfo.hpp" +#include "rtps/config.hpp" +#include "rtps/entities/Reader.hpp" +#include "rtps/entities/WriterProxy.hpp" +#include "rtps/storages/MemoryPool.hpp" + +namespace rtps { +class EsppTransport; +struct SubmessageHeartbeat; + +template class StatefulReaderT final : public Reader { +public: + StatefulReaderT() + : m_srcPort(0) + , m_transport(nullptr) {} + ~StatefulReaderT() override; + bool init(const TopicData &attributes, NetworkDriver &driver); + void newChange(const ReaderCacheChange &cacheChange) override; + bool addNewMatchedWriter(const WriterProxy &newProxy) override; + bool onNewHeartbeat(const SubmessageHeartbeat &msg, const GuidPrefix_t &remotePrefix) override; + bool onNewGapMessage(const SubmessageGap &msg, const GuidPrefix_t &remotePrefix) override; + + bool sendPreemptiveAckNack(const WriterProxy &writer) override; + +private: + Ip4Port_t m_srcPort; // TODO intended for reuse but buffer not used as such + NetworkDriver *m_transport; +}; + +using StatefulReader = StatefulReaderT; + +} // namespace rtps + +#include "StatefulReader.tpp" + +#endif // RTPS_STATEFULREADER_H diff --git a/components/rtps_embedded/include/rtps/entities/StatefulReader.tpp b/components/rtps_embedded/include/rtps/entities/StatefulReader.tpp new file mode 100644 index 000000000..96c573419 --- /dev/null +++ b/components/rtps_embedded/include/rtps/entities/StatefulReader.tpp @@ -0,0 +1,282 @@ +/* +The MIT License +Copyright (c) 2019 Lehrstuhl Informatik 11 - RWTH Aachen University +Modifications Copyright (c) 2026 ATDev +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: +The above copyright notice and this permission notice shall be included in +all copies or substantial portions of the Software. +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +THE SOFTWARE + +This file is part of embeddedRTPS. + +Author: i11 - Embedded Software, RWTH Aachen University +*/ + +#include "rtps/entities/StatefulReader.hpp" +#include "rtps/messages/MessageFactory.hpp" +#include "rtps/storages/PayloadBuffer.hpp" +#include "rtps/utils/Diagnostics.hpp" +#include "rtps/utils/Log.hpp" +#include + +#if SFR_VERBOSE && RTPS_GLOBAL_VERBOSE +#include "rtps/utils/printutils.hpp" +#define SFR_LOG(...) logger_.warn(__VA_ARGS__) +#else +#define SFR_LOG(...) \ + do { \ + } while (0) +#endif + +using rtps::Guid_t; +using rtps::GuidPrefix_t; +using rtps::PacketInfo; +using rtps::ReaderCacheChange; +using rtps::SequenceNumber_t; +using rtps::SequenceNumberSet; +using rtps::StatefulReaderT; +using rtps::SubmessageGap; +using rtps::SubmessageHeartbeat; +using rtps::TopicData; +using rtps::WriterProxy; + +template StatefulReaderT::~StatefulReaderT() {} + +template +bool StatefulReaderT::init(const TopicData &attributes, NetworkDriver &driver) { + if (!initMutex()) { + return false; + } + + m_proxies.clear(); + m_attributes = attributes; + m_transport = &driver; + m_srcPort = attributes.unicastLocator.port; + m_is_initialized_ = true; + return true; +} + +template +void StatefulReaderT::newChange(const ReaderCacheChange &cacheChange) { + if (m_callback_count == 0 || !m_is_initialized_) { + return; + } + std::lock_guard lock(m_proxies_mutex); + for (auto &proxy : m_proxies) { + if (proxy.remoteWriterGuid == cacheChange.writerGuid) { + if (proxy.expectedSN == cacheChange.sn) { + SFR_LOG("Delivering SN {}.{} | GUID {} {} {} {}", (int)cacheChange.sn.high, + (int)cacheChange.sn.low, cacheChange.writerGuid.prefix.id[0], + cacheChange.writerGuid.prefix.id[1], cacheChange.writerGuid.prefix.id[2], + cacheChange.writerGuid.prefix.id[3]); + executeCallbacks(cacheChange); + ++proxy.expectedSN; + SFR_LOG("Done processing SN {}.{}", (int)cacheChange.sn.high, (int)cacheChange.sn.low); + return; + } else { + Diagnostics::StatefulReader::sfr_unexpected_sn++; + SFR_LOG("Unexpected SN {}.{} != {}.{}, dropping! GUID {} {} {} {}", + (int)proxy.expectedSN.high, (int)proxy.expectedSN.low, (int)cacheChange.sn.high, + (int)cacheChange.sn.low, cacheChange.writerGuid.prefix.id[0], + cacheChange.writerGuid.prefix.id[1], cacheChange.writerGuid.prefix.id[2], + cacheChange.writerGuid.prefix.id[3]); + } + } + } +} + +template +bool StatefulReaderT::addNewMatchedWriter(const WriterProxy &newProxy) { +#if SFR_VERBOSE && RTPS_GLOBAL_VERBOSE + SFR_LOG("New writer added"); +#endif + return m_proxies.add(newProxy); +} + +template +bool StatefulReaderT::onNewGapMessage(const SubmessageGap &msg, + const GuidPrefix_t &remotePrefix) { + std::lock_guard lock(m_proxies_mutex); + if (!m_is_initialized_) { + return false; + } + SFR_LOG("Processing gap message {}.{} {}.{}", (int)msg.gapStart.high, + (unsigned int)msg.gapStart.low, (int)msg.gapList.base.high, + (unsigned int)msg.gapList.base.low); + + Guid_t writerProxyGuid; + writerProxyGuid.prefix = remotePrefix; + writerProxyGuid.entityId = msg.writerId; + WriterProxy *writer = getProxy(writerProxyGuid); + + if (writer == nullptr) { + +#if SFR_VERBOSE && RTPS_GLOBAL_VERBOSE + SFR_LOG("Ignore GAP. Couldn't find a matching writer"); +#endif + return false; + } + + // Case 1: We are still waiting for messages before gapStart + if (writer->expectedSN < msg.gapStart) { + PacketInfo info; + info.srcPort = m_srcPort; + info.destAddr = writer->remoteLocator.getIp4AddressBytes(); + info.destPort = writer->remoteLocator.port; + PayloadBuffer payload; + rtps::MessageFactory::addHeader(payload, m_attributes.endpointGuid.prefix); + SequenceNumber_t last_valid = msg.gapStart; + --last_valid; + auto missing_sns = writer->getMissing(writer->expectedSN, last_valid); + rtps::MessageFactory::addAckNack(payload, msg.writerId, msg.readerId, missing_sns, + writer->getNextAckNackCount(), false); + info.payload = std::move(payload.bytes); + if (info.payload.empty()) { + return false; + } + m_transport->sendPacket(info); + return true; + } + + // Case 2: We are expecting a message between [gapStart; gapList.base -1] + // Advance expectedSN beyond gapList.base + if (writer->expectedSN < msg.gapList.base) { + writer->expectedSN = msg.gapList.base; + + // writer->expectedSN++; + + // Advance expectedSN to first unset bit + for (uint32_t bit = 0; bit < SNS_MAX_NUM_BITS; writer->expectedSN++, bit++) { + if (!msg.gapList.isSet(bit)) { + break; + } + } + + return true; + + } else { + + // Case 3: We are expecting a sequence number beyond gap list base, + // check if we need to update expectedSN + auto i = msg.gapList.base; + for (uint32_t bit = 0; bit < SNS_MAX_NUM_BITS; i++, bit++) { + if (i < writer->expectedSN) { + continue; + } + + if (msg.gapList.isSet(bit)) { + writer->expectedSN++; + } else { + PacketInfo info; + info.srcPort = m_srcPort; + info.destAddr = writer->remoteLocator.getIp4AddressBytes(); + info.destPort = writer->remoteLocator.port; + PayloadBuffer payload; + rtps::MessageFactory::addHeader(payload, m_attributes.endpointGuid.prefix); + SequenceNumberSet set; + set.base = writer->expectedSN; + set.numBits = 1; + set.bitMap[0] = set.bitMap[0] |= uint32_t{1} << 31; + rtps::MessageFactory::addAckNack(payload, msg.writerId, msg.readerId, set, + writer->getNextAckNackCount(), false); + info.payload = std::move(payload.bytes); + if (info.payload.empty()) { + return false; + } + m_transport->sendPacket(info); + + return true; + } + } + + return false; + } +} + +template +bool StatefulReaderT::onNewHeartbeat(const SubmessageHeartbeat &msg, + const GuidPrefix_t &sourceGuidPrefix) { + std::lock_guard lock(m_proxies_mutex); + if (!m_is_initialized_) { + return false; + } + PacketInfo info; + info.srcPort = m_srcPort; + PayloadBuffer payload; + + Guid_t writerProxyGuid; + writerProxyGuid.prefix = sourceGuidPrefix; + writerProxyGuid.entityId = msg.writerId; + WriterProxy *writer = getProxy(writerProxyGuid); + + if (writer == nullptr) { + +#if SFR_VERBOSE && RTPS_GLOBAL_VERBOSE + SFR_LOG("Ignore heartbeat. Couldn't find a matching writer"); +#endif + return false; + } + + if (writer->expectedSN < msg.firstSN) { + SFR_LOG("expectedSN < firstSN, advancing expectedSN"); + writer->expectedSN = msg.firstSN; + } + + writer->hbCount.value = msg.count.value; + info.destAddr = writer->remoteLocator.getIp4AddressBytes(); + info.destPort = writer->remoteLocator.port; + rtps::MessageFactory::addHeader(payload, m_attributes.endpointGuid.prefix); + auto missing_sns = writer->getMissing(msg.firstSN, msg.lastSN); + bool final_flag = (missing_sns.numBits == 0); + rtps::MessageFactory::addAckNack(payload, msg.writerId, msg.readerId, missing_sns, + writer->getNextAckNackCount(), final_flag); + + SFR_LOG("Sending acknack base {} bits {}.", (int)missing_sns.base.low, (int)missing_sns.numBits); + info.payload = std::move(payload.bytes); + if (info.payload.empty()) { + return false; + } + m_transport->sendPacket(info); + return true; +} + +template +bool StatefulReaderT::sendPreemptiveAckNack(const WriterProxy &writer) { + std::lock_guard lock(m_proxies_mutex); + if (!m_is_initialized_) { + return false; + } + + PacketInfo info; + info.srcPort = m_attributes.unicastLocator.port; + info.destAddr = writer.remoteLocator.getIp4AddressBytes(); + info.destPort = writer.remoteLocator.port; + PayloadBuffer payload; + rtps::MessageFactory::addHeader(payload, m_attributes.endpointGuid.prefix); + SequenceNumberSet number_set; + number_set.base.high = 0; + number_set.base.low = 0; + number_set.numBits = 0; + rtps::MessageFactory::addAckNack(payload, writer.remoteWriterGuid.entityId, + m_attributes.endpointGuid.entityId, number_set, Count_t{1}, + false); + + SFR_LOG("Sending preemptive acknack."); + info.payload = std::move(payload.bytes); + if (info.payload.empty()) { + return false; + } + m_transport->sendPacket(info); + return true; +} diff --git a/components/rtps_embedded/include/rtps/entities/StatefulWriter.hpp b/components/rtps_embedded/include/rtps/entities/StatefulWriter.hpp new file mode 100644 index 000000000..00fef2ef4 --- /dev/null +++ b/components/rtps_embedded/include/rtps/entities/StatefulWriter.hpp @@ -0,0 +1,98 @@ +/* +The MIT License +Copyright (c) 2019 Lehrstuhl Informatik 11 - RWTH Aachen University +Modifications Copyright (c) 2026 ATDev +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: +The above copyright notice and this permission notice shall be included in +all copies or substantial portions of the Software. +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +THE SOFTWARE + +This file is part of embeddedRTPS. + +Author: i11 - Embedded Software, RWTH Aachen University +*/ + +#ifndef RTPS_STATEFULWRITER_H +#define RTPS_STATEFULWRITER_H + +#include "rtps/common/types.hpp" +#include "rtps/entities/ReaderProxy.hpp" +#include "rtps/entities/Writer.hpp" +#include "rtps/storages/HistoryCacheWithDeletion.hpp" +#include "rtps/storages/MemoryPool.hpp" +#include "task.hpp" + +#include + +namespace rtps { + +class EsppTransport; + +template class StatefulWriterT final : public Writer { +public: + StatefulWriterT() + : m_transport(nullptr) {} + ~StatefulWriterT() override; + bool init(TopicData attributes, TopicKind_t topicKind, 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 = nullptr; + + HistoryCacheWithDeletion m_history; + + /* + * Cache changes marked as disposeAfterWrite are retained for a short amount + * in case of retransmission The whole 'disposeAfterWrite' mechanisms only + * exists to allow for repeated creation and deletion of endpoints during + * operation. Otherwise the history will quickly reach its limits. Will be + * replaced with something more elegant in the future. + */ + ThreadSafeCircularBuffer m_disposeWithDelay; + void dropDisposeAfterWriteChanges(); + + 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..cc2804fff --- /dev/null +++ b/components/rtps_embedded/include/rtps/entities/StatefulWriter.tpp @@ -0,0 +1,560 @@ +/* +The MIT License +Copyright (c) 2019 Lehrstuhl Informatik 11 - RWTH Aachen University +Modifications Copyright (c) 2026 ATDev +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: +The above copyright notice and this permission notice shall be included in +all copies or substantial portions of the Software. +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +THE SOFTWARE + +This file is part of embeddedRTPS. + +Author: i11 - Embedded Software, RWTH Aachen University +*/ + +#include "rtps/entities/StatefulWriter.hpp" +#include "rtps/messages/MessageFactory.hpp" +#include "rtps/messages/MessageTypes.hpp" +#include "rtps/storages/PayloadBuffer.hpp" +#include "rtps/utils/Log.hpp" +#include +#include +#include +#include +#include +#include + +using rtps::CacheChange; +using rtps::GuidPrefix_t; +using rtps::ReaderProxy; +using rtps::SequenceNumber_t; +using rtps::StatefulWriterT; +using rtps::SubmessageAckNack; + +#if SFW_VERBOSE && RTPS_GLOBAL_VERBOSE +#include "rtps/utils/printutils.hpp" +#define SFW_LOG(...) logger_.warn(__VA_ARGS__) +#else +#define SFW_LOG(...) \ + do { \ + } while (0) +#endif + +template StatefulWriterT::~StatefulWriterT() { + 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}; + + // 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); + + /* + * Use case: deletion of local endpoints + * -> send Data Message with Disposed Flag set + * -> Set respective SEDP CacheChange as NOT_ALIVE_DISPOSED after + * transmission to proxies + * -> onAckNack will send Gap Messages to skip deleted local endpoints + * during SEDP + */ + if (next->disposeAfterWrite) { + SFW_LOG("Dispose after write msg sent to {} proxies", (int)i); + next->sentTime = std::chrono::steady_clock::now(); + if (!m_disposeWithDelay.copyElementIntoBuffer(next->sequenceNumber)) { + SFW_LOG("Failed to enqueue dispose after write!"); + m_history.dropChange(next->sequenceNumber); + } else { + SFW_LOG("Delayed dispose scheduled for sn {} {}", (int)next->sequenceNumber.high, + (int)next->sequenceNumber.low); + } + } + + ++m_nextSequenceNumberToSend; + SFW_LOG("HB from progress"); + sendHeartBeat(); + + } else { + SFW_LOG("Couldn't get a CacheChange with SN ({},{})", m_nextSequenceNumberToSend.high, + m_nextSequenceNumberToSend.low); + } +} + +template void StatefulWriterT::setAllChangesToUnsent() { + INIT_GUARD() + std::lock_guard lock(m_mutex); + + m_nextSequenceNumberToSend = m_history.getCurrentSeqNumMin(); + + if (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; + } + + auto proxy_it = std::find_if(m_proxies.begin(), m_proxies.end(), [&](const auto &proxy) { + return proxy.remoteReaderGuid.prefix == sourceGuidPrefix && + proxy.remoteReaderGuid.entityId == msg.readerId; + }); + ReaderProxy *reader = (proxy_it != m_proxies.end()) ? &(*proxy_it) : nullptr; + + if (reader == nullptr) { +#if SFW_VERBOSE && RTPS_GLOBAL_VERBOSE + SFW_LOG("No proxy found with id: "); + printEntityId(msg.readerId); + SFW_LOG(" Dropping acknack.\n"); +#endif + return; + } + + reader->ackNackCount = msg.count; + reader->finalFlag = msg.header.finalFlag(); + reader->lastAckNackSequenceNumber = msg.readerSNState.base; + + rtps::SequenceNumber_t nextSN = msg.readerSNState.base; + + // Preemptive ack nack + if (nextSN.low == 0 && nextSN.high == 0) { + SFW_LOG("Received preemptive acknack, sending heartbeat."); + sendHeartBeat(); + return; + } + + if (m_history.isEmpty()) { + // We have never sent anything. Do not immediately respond with another + // heartbeat here, otherwise reader/writer can get stuck in HB<->ACKNACK + // ping-pong. Periodic heartbeat still handles liveliness. + if (m_history.getLastUsedSequenceNumber() == rtps::SequenceNumber_t{0, 0}) { + SFW_LOG("Ignoring acknack while history is empty and no samples were sent yet."); + return; + } else { + // No data but we have sent something in the past -> GapStart = + // readerSNState.base, NextValid = lastUsedSequenceNumber+1 + rtps::SequenceNumber_t nextValid = m_history.getLastUsedSequenceNumber(); + ++nextValid; + sendGap(*reader, msg.readerSNState.base, nextValid); + } + + return; + } + + // Requesting smaller SN than minimum sequence number -> sendGap + if (msg.readerSNState.base < m_history.getCurrentSeqNumMin()) { + sendGap(*reader, msg.readerSNState.base, m_history.getCurrentSeqNumMin()); + return; + } + + SFW_LOG("Received non-preemptive acknack with {} bits set.", msg.readerSNState.numBits); + for (uint32_t i = 0; + i < msg.readerSNState.numBits && nextSN <= m_history.getLastUsedSequenceNumber(); + ++i, ++nextSN) { + + if (msg.readerSNState.isSet(i)) { + + SFW_LOG("Looking for change {} | Bit {}", nextSN.low, i); + const rtps::CacheChange *cache = m_history.getChangeBySN(nextSN); + + // We still have the cache, send DATA + if (cache != nullptr) { + if (cache->disposeAfterWrite) { + SFW_LOG("Serving from dispose-after-write cache"); + } + sendData(*reader, cache); + } else { + SFW_LOG("> Change not found, search for next valid SN {}", nextSN.low); + // Cache not found, look for next valid SN + rtps::SequenceNumber_t gapBegin = nextSN; + rtps::CacheChange *nextValidChange = nullptr; + uint32_t j = i + 1; + for (++nextSN; nextSN <= m_history.getLastUsedSequenceNumber(); ++nextSN, ++j) { + nextValidChange = m_history.getChangeBySN(nextSN); + if (nextValidChange != nullptr) { + break; + } + } + if (nextValidChange == nullptr) { + sendGap(*reader, gapBegin, nextSN); + return; + } else { + sendGap(*reader, gapBegin, nextValidChange->sequenceNumber); + } + // sendData(nullptr, nextValidChange); + nextSN = nextValidChange->sequenceNumber; + --nextSN; + i = --j; + } + } + } +} + +template +bool rtps::StatefulWriterT::removeFromHistory(const SequenceNumber_t &s) { + std::lock_guard lock(m_mutex); + return m_history.dropChange(s); +} + +template +bool StatefulWriterT::sendData(const ReaderProxy &reader, const CacheChange *next) { + INIT_GUARD() + // TODO smarter packaging, e.g. create a message struct and serialize once. + + PacketInfo info; + info.srcPort = m_srcPort; + PayloadBuffer payload; + + MessageFactory::addHeader(payload, m_attributes.endpointGuid.prefix); + MessageFactory::addSubMessageTimeStamp(payload); + + // Just usable for IPv4 + const LocatorIPv4 &locator = reader.remoteLocator; + + info.destAddr = locator.getIp4AddressBytes(); + info.destPort = (Ip4Port_t)locator.port; + + MessageFactory::addSubMessageData(payload, next->data, next->inLineQoS, next->sequenceNumber, + m_attributes.endpointGuid.entityId, + reader.remoteReaderGuid.entityId); + info.payload = std::move(payload.bytes); + if (info.payload.empty()) { + return false; + } + m_transport->sendPacket(info); + + return true; +} + +template +void StatefulWriterT::sendGap(const ReaderProxy &reader, + const SequenceNumber_t &firstMissing, + const SequenceNumber_t &nextValid) { + INIT_GUARD() + // TODO smarter packaging, e.g. create a message struct and serialize once. + + PacketInfo info; + info.srcPort = m_srcPort; + PayloadBuffer payload; + + MessageFactory::addHeader(payload, m_attributes.endpointGuid.prefix); + MessageFactory::addSubMessageTimeStamp(payload); + + // Just usable for IPv4 + const LocatorIPv4 &locator = reader.remoteLocator; + + info.destAddr = locator.getIp4AddressBytes(); + info.destPort = (Ip4Port_t)locator.port; + + MessageFactory::addSubmessageGap(payload, m_attributes.endpointGuid.entityId, + reader.remoteReaderGuid.entityId, firstMissing, nextValid); + info.payload = std::move(payload.bytes); + if (info.payload.empty()) { + return; + } + m_transport->sendPacket(info); +} + +template +bool StatefulWriterT::sendDataWRMulticast(const ReaderProxy &reader, + const CacheChange *next) { + INIT_GUARD() + + if (reader.useMulticast || reader.suppressUnicast == false) { + PacketInfo info; + info.srcPort = m_srcPort; + PayloadBuffer payload; + + MessageFactory::addHeader(payload, m_attributes.endpointGuid.prefix); + MessageFactory::addSubMessageTimeStamp(payload); + + // Decide whether to use multicast or unicast. + if (reader.useMulticast) { + const LocatorIPv4 &locator = reader.remoteMulticastLocator; + info.destAddr = locator.getIp4AddressBytes(); + info.destPort = (Ip4Port_t)locator.port; + } else { + const LocatorIPv4 &locator = reader.remoteLocator; + info.destAddr = locator.getIp4AddressBytes(); + info.destPort = (Ip4Port_t)locator.port; + } + + EntityId_t reid; + if (reader.useMulticast) { + reid = ENTITYID_UNKNOWN; + } else { + reid = reader.remoteReaderGuid.entityId; + } + + MessageFactory::addSubMessageData(payload, next->data, next->inLineQoS, next->sequenceNumber, + m_attributes.endpointGuid.entityId, reid); + + info.payload = std::move(payload.bytes); + if (info.payload.empty()) { + return false; + } + m_transport->sendPacket(info); + } + return true; +} + +template void StatefulWriterT::sendHeartBeatLoop() { + m_thread_running = true; + while (m_running) { + SFW_LOG("HB from loop"); + sendHeartBeat(); + dropDisposeAfterWriteChanges(); + const auto pending_ack_proxy = + std::find_if(m_proxies.begin(), m_proxies.end(), [&](const auto &proxy) { + return proxy.lastAckNackSequenceNumber < m_nextSequenceNumberToSend; + }); + const bool unconfirmed_changes = pending_ack_proxy != m_proxies.end(); + + // 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.hpp b/components/rtps_embedded/include/rtps/entities/StatelessReader.hpp new file mode 100644 index 000000000..4455f7c3b --- /dev/null +++ b/components/rtps_embedded/include/rtps/entities/StatelessReader.hpp @@ -0,0 +1,43 @@ +/* +The MIT License +Copyright (c) 2019 Lehrstuhl Informatik 11 - RWTH Aachen University +Modifications Copyright (c) 2026 ATDev +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: +The above copyright notice and this permission notice shall be included in +all copies or substantial portions of the Software. +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +THE SOFTWARE + +This file is part of embeddedRTPS. + +Author: i11 - Embedded Software, RWTH Aachen University +*/ + +#ifndef RTPS_STATELESSREADER_H +#define RTPS_STATELESSREADER_H + +#include "rtps/entities/Reader.hpp" + +namespace rtps { +class StatelessReader final : public Reader { +public: + bool init(const TopicData &attributes); + void newChange(const ReaderCacheChange &cacheChange) override; + bool onNewHeartbeat(const SubmessageHeartbeat &msg, const GuidPrefix_t &remotePrefix) override; + bool addNewMatchedWriter(const WriterProxy &newProxy) override; + bool onNewGapMessage(const SubmessageGap &msg, const GuidPrefix_t &remotePrefix) override; +}; + +} // namespace rtps + +#endif // RTPS_STATELESSREADER_H diff --git a/components/rtps_embedded/include/rtps/entities/StatelessWriter.hpp b/components/rtps_embedded/include/rtps/entities/StatelessWriter.hpp new file mode 100644 index 000000000..576fcf3b8 --- /dev/null +++ b/components/rtps_embedded/include/rtps/entities/StatelessWriter.hpp @@ -0,0 +1,69 @@ +/* +The MIT License +Copyright (c) 2019 Lehrstuhl Informatik 11 - RWTH Aachen University +Modifications Copyright (c) 2026 ATDev +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: +The above copyright notice and this permission notice shall be included in +all copies or substantial portions of the Software. +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +THE SOFTWARE + +This file is part of embeddedRTPS. + +Author: i11 - Embedded Software, RWTH Aachen University +*/ + +#ifndef RTPS_RTPSWRITER_H +#define RTPS_RTPSWRITER_H + +#include "rtps/common/types.hpp" +#include "rtps/config.hpp" +#include "rtps/entities/Writer.hpp" +#include "rtps/storages/MemoryPool.hpp" +#include "rtps/storages/SimpleHistoryCache.hpp" + +namespace rtps { + +class EsppTransport; + +template class StatelessWriterT : public Writer { +public: + StatelessWriterT() + : m_transport(nullptr) {} + ~StatelessWriterT() override; + bool init(TopicData attributes, TopicKind_t topicKind, 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..19154de93 --- /dev/null +++ b/components/rtps_embedded/include/rtps/entities/StatelessWriter.tpp @@ -0,0 +1,210 @@ +/* +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.hpp" +#include "rtps/messages/MessageFactory.hpp" +#include "rtps/storages/PayloadBuffer.hpp" +#include "rtps/utils/Log.hpp" +#include "rtps/utils/udpUtils.hpp" +#include + +using rtps::CacheChange; +using rtps::GuidPrefix_t; +using rtps::SequenceNumber_t; +using rtps::StatelessWriterT; +using rtps::SubmessageAckNack; + +#if SLW_VERBOSE && RTPS_GLOBAL_VERBOSE +#include "rtps/utils/printutils.hpp" +#define SLW_LOG(...) logger_.warn(__VA_ARGS__) +#else +#define SLW_LOG(...) \ + do { \ + } while (0) +#endif + +template StatelessWriterT::~StatelessWriterT() { + // if(sys_mutex_valid(&m_mutex)){ + // sys_mutex_free(&m_mutex); + // } +} + +template +bool StatelessWriterT::init(TopicData attributes, TopicKind_t topicKind, + 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.hpp b/components/rtps_embedded/include/rtps/entities/Writer.hpp new file mode 100644 index 000000000..99bf5c571 --- /dev/null +++ b/components/rtps_embedded/include/rtps/entities/Writer.hpp @@ -0,0 +1,115 @@ +/* +The MIT License +Copyright (c) 2019 Lehrstuhl Informatik 11 - RWTH Aachen University +Modifications Copyright (c) 2026 ATDev +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: +The above copyright notice and this permission notice shall be included in +all copies or substantial portions of the Software. +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +THE SOFTWARE + +This file is part of embeddedRTPS. + +Author: i11 - Embedded Software, RWTH Aachen University +*/ + +#ifndef RTPS_WRITER_H +#define RTPS_WRITER_H + +#include "base_component.hpp" +#include "rtps/ThreadPool.hpp" +#include "rtps/discovery/TopicData.hpp" +#include "rtps/entities/ReaderProxy.hpp" +#include "rtps/storages/CacheChange.hpp" +#include "rtps/storages/MemoryPool.hpp" + +#include +#include + +#ifdef DEBUG_BUILD +#define COMPILE_INIT_GUARD +#endif + +#ifdef COMPILE_INIT_GUARD +#define INIT_GUARD() \ + if (!m_is_initialized_) { \ + logger_.error("Using uninitialized endpoint"); \ + while (1) { \ + } \ + } \ + } +#else +#define INIT_GUARD() // +#endif + +namespace rtps { + +class Writer : public espp::BaseComponent { +public: + TopicData m_attributes; + virtual bool addNewMatchedReader(const ReaderProxy &newProxy); + virtual bool removeProxy(const Guid_t &guid); + virtual void removeAllProxiesOfParticipant(const GuidPrefix_t &guidPrefix); + virtual void reset() = 0; + virtual const CacheChange *newChange(ChangeKind_t kind, const uint8_t *data, DataSize_t size); + + //! Executes required steps like sending packets. Intended to be called by + //! worker threads + virtual void progress() = 0; + + virtual bool removeFromHistory(const SequenceNumber_t &s) = 0; + virtual void setAllChangesToUnsent() = 0; + virtual void onNewAckNack(const SubmessageAckNack &msg, const GuidPrefix_t &sourceGuidPrefix) = 0; + + using dumpProxyCallback = void (*)(const Writer *writer, const ReaderProxy &, void *arg); + + int dumpAllProxies(dumpProxyCallback target, void *arg); + + bool isInitialized(); + std::uint32_t getProxiesCount(); + + void setSEDPSequenceNumber(const SequenceNumber_t &sn); + const SequenceNumber_t &getSEDPSequenceNumber(); + + bool isBuiltinEndpoint(); + +protected: + Writer(); + SequenceNumber_t m_sedp_sequence_number; + + std::recursive_mutex m_mutex; + 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.hpp b/components/rtps_embedded/include/rtps/entities/WriterProxy.hpp new file mode 100644 index 000000000..d3f6f0edc --- /dev/null +++ b/components/rtps_embedded/include/rtps/entities/WriterProxy.hpp @@ -0,0 +1,80 @@ +/* +The MIT License +Copyright (c) 2019 Lehrstuhl Informatik 11 - RWTH Aachen University +Modifications Copyright (c) 2026 ATDev +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: +The above copyright notice and this permission notice shall be included in +all copies or substantial portions of the Software. +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +THE SOFTWARE + +This file is part of embeddedRTPS. + +Author: i11 - Embedded Software, RWTH Aachen University +*/ + +#ifndef RTPS_WRITERPROXY_H +#define RTPS_WRITERPROXY_H + +#include "rtps/common/types.hpp" +#include + +namespace rtps { +struct WriterProxy { + Guid_t remoteWriterGuid; + SequenceNumber_t expectedSN; + Count_t ackNackCount; + Count_t hbCount; + bool is_reliable = false; + LocatorIPv4 remoteLocator; + + WriterProxy() = default; + + WriterProxy(const Guid_t &guid, const LocatorIPv4 &loc, bool reliable) + : remoteWriterGuid(guid) + , expectedSN(SequenceNumber_t{0, 1}) + , ackNackCount{1} + , hbCount{0} + , is_reliable(reliable) + , remoteLocator(loc) {} + + // For now, we don't store any packets, so we just request all starting from + // the next expected + SequenceNumberSet getMissing(const SequenceNumber_t &firstAvail, + const SequenceNumber_t &lastAvail) const { + SequenceNumberSet set; + if (lastAvail < expectedSN) { + set.base = expectedSN; + set.numBits = 0; + } else { + set.base = expectedSN; + SequenceNumber_t i; + uint32_t bit; + for (bit = 0, i = expectedSN; i <= lastAvail && bit < SNS_MAX_NUM_BITS; i++, bit++) { + set.bitMap[0] |= uint32_t{1} << (31 - bit); + set.numBits++; + } + } + + return set; + } + + Count_t getNextAckNackCount() { + const Count_t tmp = ackNackCount; + ++ackNackCount.value; + return tmp; + } +}; +} // namespace rtps + +#endif // RTPS_WRITERPROXY_H diff --git a/components/rtps_embedded/include/rtps/messages/MessageFactory.hpp b/components/rtps_embedded/include/rtps/messages/MessageFactory.hpp new file mode 100644 index 000000000..0507b6fec --- /dev/null +++ b/components/rtps_embedded/include/rtps/messages/MessageFactory.hpp @@ -0,0 +1,207 @@ +/* +The MIT License +Copyright (c) 2019 Lehrstuhl Informatik 11 - RWTH Aachen University +Modifications Copyright (c) 2026 ATDev +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: +The above copyright notice and this permission notice shall be included in +all copies or substantial portions of the Software. +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +THE SOFTWARE + +This file is part of embeddedRTPS. + +Author: i11 - Embedded Software, RWTH Aachen University +*/ + +// Copyright 2023 Apex.AI, Inc. +// All rights reserved. + +#ifndef RTPS_MESSAGEFACTORY_H +#define RTPS_MESSAGEFACTORY_H + +#include "rtps/common/types.hpp" +#include "rtps/config.hpp" +#include "rtps/messages/MessageTypes.hpp" +#include "rtps/utils/sysFunctions.hpp" + +#include +#include + +namespace rtps { +namespace MessageFactory { +const std::array PROTOCOL_TYPE{'R', 'T', 'P', 'S'}; +const uint8_t numBytesUntilEndOfLength = 4; // The first bytes incl. submessagelength don't count + +template void addHeader(Buffer &buffer, const GuidPrefix_t &guidPrefix) { + + Header header; + header.protocolName = PROTOCOL_TYPE; + header.protocolVersion = PROTOCOLVERSION; + header.vendorId = Config::VENDOR_ID; + header.guidPrefix = guidPrefix; + + serializeMessage(buffer, header); +} + +template bool addSubMessageInfoDST(Buffer &buffer, GuidPrefix_t &dst) { + SubmessageInfoDST msg; + msg.header.submessageId = SubmessageKind::INFO_DST; + +#if IS_LITTLE_ENDIAN + msg.header.flags = FLAG_LITTLE_ENDIAN; +#else + msg.header.flags = FLAG_BIG_ENDIAN; +#endif + + msg.header.octetsToNextHeader = sizeof(GuidPrefix_t); + msg.guidPrefix = dst; + + return serializeMessage(buffer, msg); +} + +template void addSubMessageTimeStamp(Buffer &buffer, bool setInvalid = false) { + SubmessageHeader header; + header.submessageId = SubmessageKind::INFO_TS; + +#if IS_LITTLE_ENDIAN + header.flags = FLAG_LITTLE_ENDIAN; +#else + header.flags = FLAG_BIG_ENDIAN; +#endif + + if (setInvalid) { + header.flags |= FLAG_INVALIDATE; + header.octetsToNextHeader = 0; + } else { + header.octetsToNextHeader = sizeof(Time_t); + } + + serializeMessage(buffer, header); + + if (!setInvalid) { + buffer.reserve(header.octetsToNextHeader); + Time_t now = getCurrentTimeStamp(); + buffer.append(reinterpret_cast(&now.seconds), sizeof(Time_t::seconds)); + buffer.append(reinterpret_cast(&now.fraction), sizeof(Time_t::fraction)); + } +} + +template +void addSubMessageData(Buffer &buffer, const PayloadBuffer &filledPayload, bool containsInlineQos, + const SequenceNumber_t &SN, const EntityId_t &writerID, + const EntityId_t &readerID) { + SubmessageData msg; + msg.header.submessageId = SubmessageKind::DATA; +#if IS_LITTLE_ENDIAN + msg.header.flags = FLAG_LITTLE_ENDIAN; +#else + msg.header.flags = FLAG_BIG_ENDIAN; +#endif + + msg.header.octetsToNextHeader = + SubmessageData::getRawSize() + filledPayload.spaceUsed() - numBytesUntilEndOfLength; + + if (containsInlineQos) { + msg.header.flags |= FLAG_INLINE_QOS; + } + if (filledPayload.isValid()) { + msg.header.flags |= FLAG_DATA_PAYLOAD; + } + + msg.writerSN = SN; + msg.extraFlags = 0; + msg.readerId = readerID; + msg.writerId = writerID; + + constexpr uint16_t octetsToInlineQoS = 4 + 4 + 8; // EntityIds + SequenceNumber + msg.octetsToInlineQos = octetsToInlineQoS; + + serializeMessage(buffer, msg); + + if (filledPayload.isValid()) { + buffer.append(filledPayload); + } +} + +template +void addHeartbeat(Buffer &buffer, EntityId_t writerId, EntityId_t readerId, + SequenceNumber_t firstSN, SequenceNumber_t lastSN, Count_t count) { + SubmessageHeartbeat subMsg; + subMsg.header.submessageId = SubmessageKind::HEARTBEAT; + subMsg.header.octetsToNextHeader = SubmessageHeartbeat::getRawSize() - numBytesUntilEndOfLength; +#if IS_LITTLE_ENDIAN + subMsg.header.flags = FLAG_LITTLE_ENDIAN; +#else + subMsg.header.flags = FLAG_BIG_ENDIAN; +#endif + // Force response by not setting final flag. + + subMsg.writerId = writerId; + subMsg.readerId = readerId; + subMsg.firstSN = firstSN; + subMsg.lastSN = lastSN; + subMsg.count = count; + + serializeMessage(buffer, subMsg); +} + +template +void addAckNack(Buffer &buffer, EntityId_t writerId, EntityId_t readerId, + SequenceNumberSet readerSNState, Count_t count, bool final_flag) { + SubmessageAckNack subMsg; + subMsg.header.submessageId = SubmessageKind::ACKNACK; +#if IS_LITTLE_ENDIAN + subMsg.header.flags = FLAG_LITTLE_ENDIAN; +#else + subMsg.header.flags = FLAG_BIG_ENDIAN; +#endif + if (final_flag) { + subMsg.header.flags |= FLAG_FINAL; // For now, we don't want any response + } else { + subMsg.header.flags &= ~FLAG_FINAL; // Send future heartbeats, even if no change occured + } + subMsg.header.octetsToNextHeader = + SubmessageAckNack::getRawSize(readerSNState) - numBytesUntilEndOfLength; + + subMsg.writerId = writerId; + subMsg.readerId = readerId; + subMsg.readerSNState = readerSNState; + subMsg.count = count; + + serializeMessage(buffer, subMsg); +} + +template +void addSubmessageGap(Buffer &buffer, EntityId_t writerId, EntityId_t readerId, + const SequenceNumber_t &firstMissing, const SequenceNumber_t &nextValid) { + SubmessageGap subMsg; + subMsg.header.submessageId = SubmessageKind::GAP; +#if IS_LITTLE_ENDIAN + subMsg.header.flags = FLAG_LITTLE_ENDIAN; +#else + subMsg.header.flags = FLAG_BIG_ENDIAN; +#endif + subMsg.header.octetsToNextHeader = 32; + + subMsg.writerId = writerId; + subMsg.readerId = readerId; + subMsg.gapStart = firstMissing; + subMsg.gapList.base = nextValid; + subMsg.gapList.numBits = 0; + + serializeMessage(buffer, subMsg); +} +} // namespace MessageFactory +} // namespace rtps + +#endif // RTPS_MESSAGEFACTORY_H diff --git a/components/rtps_embedded/include/rtps/messages/MessageReceiver.hpp b/components/rtps_embedded/include/rtps/messages/MessageReceiver.hpp new file mode 100644 index 000000000..c0ec3e78d --- /dev/null +++ b/components/rtps_embedded/include/rtps/messages/MessageReceiver.hpp @@ -0,0 +1,78 @@ +/* +The MIT License +Copyright (c) 2019 Lehrstuhl Informatik 11 - RWTH Aachen University +Modifications Copyright (c) 2026 ATDev +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: +The above copyright notice and this permission notice shall be included in +all copies or substantial portions of the Software. +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +THE SOFTWARE + +This file is part of embeddedRTPS. + +Author: i11 - Embedded Software, RWTH Aachen University +*/ + +#ifndef RTPS_MESSAGERECEIVER_H +#define RTPS_MESSAGERECEIVER_H + +#include "base_component.hpp" +#include "rtps/common/types.hpp" +#include "rtps/config.hpp" +#include "rtps/discovery/BuiltInEndpoints.hpp" +#include + +namespace rtps { +class Reader; +class Writer; +class Participant; +class MessageProcessingInfo; + +struct MessageSourceState { + GuidPrefix_t sourceGuidPrefix = GUIDPREFIX_UNKNOWN; + ProtocolVersion_t sourceVersion = PROTOCOLVERSION; + VendorId_t sourceVendor = VENDOR_UNKNOWN; + bool haveTimeStamp = false; +}; + +class MessageReceiver : public espp::BaseComponent { +public: + explicit MessageReceiver(Participant *part); + + bool processMessage(const uint8_t *data, DataSize_t size); + +private: + Participant *mp_part; + + // TODO make msgInfo a member + // This probably make processing faster, as no parameter needs to be passed + // around However, we need to make sure data is set to nullptr after + // processMsg to make sure we don't access it again afterwards. + /** + * Check header for validity, modifies the state of the receiver and + * adjusts the position of msgInfo accordingly + */ + bool processGapSubmessage(MessageProcessingInfo &msgInfo, const MessageSourceState &sourceState); + bool processHeader(MessageProcessingInfo &msgInfo, MessageSourceState &sourceState); + bool processSubmessage(MessageProcessingInfo &msgInfo, const SubmessageHeader &submsgHeader, + const MessageSourceState &sourceState); + bool processDataSubmessage(MessageProcessingInfo &msgInfo, const SubmessageHeader &submsgHeader, + const MessageSourceState &sourceState); + bool processHeartbeatSubmessage(MessageProcessingInfo &msgInfo, + const MessageSourceState &sourceState); + bool processAckNackSubmessage(MessageProcessingInfo &msgInfo, + const MessageSourceState &sourceState); +}; +} // namespace rtps + +#endif // RTPS_MESSAGERECEIVER_H diff --git a/components/rtps_embedded/include/rtps/messages/MessageTypes.hpp b/components/rtps_embedded/include/rtps/messages/MessageTypes.hpp new file mode 100644 index 000000000..0f2a84367 --- /dev/null +++ b/components/rtps_embedded/include/rtps/messages/MessageTypes.hpp @@ -0,0 +1,415 @@ +/* +The MIT License +Copyright (c) 2019 Lehrstuhl Informatik 11 - RWTH Aachen University +Modifications Copyright (c) 2026 ATDev +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: +The above copyright notice and this permission notice shall be included in +all copies or substantial portions of the Software. +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +THE SOFTWARE + +This file is part of embeddedRTPS. + +Author: i11 - Embedded Software, RWTH Aachen University +*/ + +#ifndef RTPS_MESSAGES_H +#define RTPS_MESSAGES_H + +#include "rtps/common/types.hpp" + +#include +#include + +namespace rtps { + +#if defined(_MSC_VER) +#define RTPS_EMBEDDED_PACKED +#pragma pack(push, 1) +#else +#define RTPS_EMBEDDED_PACKED __attribute__((packed)) +#endif + +namespace SMElement { +// TODO endianess +enum ParameterId : uint16_t { + PID_PAD = 0x0000, + PID_SENTINEL = 0x0001, + PID_USER_DATA = 0x002c, + PID_TOPIC_NAME = 0x0005, + PID_TYPE_NAME = 0x0007, + PID_GROUP_DATA = 0x002d, + PID_TOPIC_DATA = 0x002e, + PID_DURABILITY = 0x001d, + PID_DURABILITY_SERVICE = 0x001e, + PID_DEADLINE = 0x0023, + PID_LATENCY_BUDGET = 0x0027, + PID_LIVELINESS = 0x001b, + PID_RELIABILITY = 0x001a, + PID_LIFESPAN = 0x002b, + PID_DESTINATION_ORDER = 0x0025, + PID_HISTORY = 0x0040, + PID_RESOURCE_LIMITS = 0x0041, + PID_OWNERSHIP = 0x001f, + PID_OWNERSHIP_STRENGTH = 0x0006, + PID_PRESENTATION = 0x0021, + PID_PARTITION = 0x0029, + PID_TIME_BASED_FILTER = 0x0004, + PID_TRANSPORT_PRIORITY = 0x0049, + PID_PROTOCOL_VERSION = 0x0015, + PID_VENDORID = 0x0016, + PID_UNICAST_LOCATOR = 0x002f, + PID_MULTICAST_LOCATOR = 0x0030, + PID_MULTICAST_IPADDRESS = 0x0011, + PID_DEFAULT_UNICAST_LOCATOR = 0x0031, + PID_DEFAULT_MULTICAST_LOCATOR = 0x0048, + PID_METATRAFFIC_UNICAST_LOCATOR = 0x0032, + PID_METATRAFFIC_MULTICAST_LOCATOR = 0x0033, + PID_DEFAULT_UNICAST_IPADDRESS = 0x000c, + PID_DEFAULT_UNICAST_PORT = 0x000e, + PID_METATRAFFIC_UNICAST_IPADDRESS = 0x0045, + PID_METATRAFFIC_UNICAST_PORT = 0x000d, + PID_METATRAFFIC_MULTICAST_IPADDRESS = 0x000b, + PID_METATRAFFIC_MULTICAST_PORT = 0x0046, + PID_EXPECTS_INLINE_QOS = 0x0043, + PID_PARTICIPANT_MANUAL_LIVELINESS_COUNT = 0x0034, + PID_PARTICIPANT_BUILTIN_ENDPOINTS = 0x0044, + PID_PARTICIPANT_LEASE_DURATION = 0x0002, + PID_CONTENT_FILTER_PROPERTY = 0x0035, + PID_PARTICIPANT_GUID = 0x0050, + PID_PARTICIPANT_ENTITYID = 0x0051, + PID_GROUP_GUID = 0x0052, + PID_GROUP_ENTITYID = 0x0053, + PID_BUILTIN_ENDPOINT_SET = 0x0058, + PID_PROPERTY_LIST = 0x0059, + PID_ENDPOINT_GUID = 0x005a, + PID_TYPE_MAX_SIZE_SERIALIZED = 0x0060, + PID_ENTITY_NAME = 0x0062, + PID_KEY_HASH = 0x0070, + PID_STATUS_INFO = 0x0071 +}; + +enum BuildInEndpointSet : uint32_t { + DISC_BIE_PARTICIPANT_ANNOUNCER = 1 << 0, + DISC_BIE_PARTICIPANT_DETECTOR = 1 << 1, + DISC_BIE_PUBLICATION_ANNOUNCER = 1 << 2, + DISC_BIE_PUBLICATION_DETECTOR = 1 << 3, + DISC_BIE_SUBSCRIPTION_ANNOUNCER = 1 << 4, + DISC_BIE_SUBSCRIPTION_DETECTOR = 1 << 5, + + DISC_BIE_PARTICIPANT_PROXY_ANNOUNCER = 1 << 6, + DISC_BIE_PARTICIPANT_PROXY_DETECTOR = 1 << 7, + DISC_BIE_PARTICIPANT_STATE_ANNOUNCER = 1 << 8, + DISC_BIE_PARTICIPANT_STATE_DETECTOR = 1 << 9, + + BIE_PARTICIPANT_MESSAGE_DATA_WRITER = 1 << 10, + BIE_PARTICIPANT_MESSAGE_DATA_READER = 1 << 11, +}; + +// TODO endianess + +const std::array SCHEME_CDR_LE{0x00, 0x01}; +const std::array SCHEME_PL_CDR_LE{0x00, 0x03}; + +struct ParameterList_t { + ParameterId pid; + uint16_t length; + // Values follow +} RTPS_EMBEDDED_PACKED; +} // namespace SMElement + +enum class SubmessageKind : uint8_t { + PAD = 0x01, /* Pad */ + ACKNACK = 0x06, /* AckNack */ + HEARTBEAT = 0x07, /* Heartbeat */ + GAP = 0x08, /* Gap */ + INFO_TS = 0x09, /* InfoTimestamp */ + INFO_SRC = 0x0c, /* InfoSource */ + INFO_REPLY_IP4 = 0x0d, /* InfoReplyIp4 */ + INFO_DST = 0x0e, /* InfoDestination */ + INFO_REPLY = 0x0f, /* InfoReply */ + NACK_FRAG = 0x12, /* NackFrag */ + HEARTBEAT_FRAG = 0x13, /* HeartbeatFrag */ + DATA = 0x15, /* Data */ + DATA_FRAG = 0x16 /* DataFrag */ +}; + +enum SubMessageFlag : uint8_t { + FLAG_ENDIANESS = (1 << 0), + FLAG_BIG_ENDIAN = 0, + FLAG_LITTLE_ENDIAN = (1 << 0), + FLAG_INVALIDATE = (1 << 1), + FLAG_INLINE_QOS = (1 << 1), + FLAG_NO_PAYLOAD = 0, + FLAG_DATA_PAYLOAD = (1 << 2), + FLAG_FINAL = (1 << 1), + FLAG_HB_LIVELINESS = (1 << 2) +}; + +const std::array RTPS_PROTOCOL_NAME = {'R', 'T', 'P', 'S'}; +struct Header { + std::array protocolName; + ProtocolVersion_t protocolVersion; + VendorId_t vendorId; + GuidPrefix_t guidPrefix; + static constexpr uint16_t getRawSize() { + return sizeof(std::array) + sizeof(ProtocolVersion_t) + sizeof(VendorId_t) + + sizeof(GuidPrefix_t); + } +}; + +struct SubmessageHeader { + SubmessageKind submessageId; + uint8_t flags; + uint16_t octetsToNextHeader; + static constexpr uint16_t getRawSize() { + return sizeof(SubmessageKind) + sizeof(uint8_t) + sizeof(uint16_t); + } + + bool finalFlag() const { return (flags & (SubMessageFlag::FLAG_FINAL)); } +}; + +struct SubmessageData { + SubmessageHeader header; + uint16_t extraFlags; + uint16_t octetsToInlineQos; + EntityId_t readerId; + EntityId_t writerId; + SequenceNumber_t writerSN; + static constexpr uint16_t getRawSize() { + return SubmessageHeader::getRawSize() + sizeof(uint16_t) + sizeof(uint16_t) + + (2 * 3 + 2 * 1) // EntityID + + sizeof(SequenceNumber_t); + } +}; + +struct SubmessageHeartbeat { + SubmessageHeader header; + EntityId_t readerId; + EntityId_t writerId; + SequenceNumber_t firstSN; + SequenceNumber_t lastSN; + Count_t count; + static constexpr uint16_t getRawSize() { + return SubmessageHeader::getRawSize() + (2 * 3 + 2 * 1) // EntityID + + 2 * sizeof(SequenceNumber_t) + sizeof(Count_t); + } +}; + +struct SubmessageInfoDST { + SubmessageHeader header; + GuidPrefix_t guidPrefix; + + static constexpr uint16_t getRawSize() { + return SubmessageHeader::getRawSize() + (sizeof(guidPrefix)); + } +}; + +struct SubmessageGap { + SubmessageHeader header; + EntityId_t readerId; + EntityId_t writerId; + SequenceNumber_t gapStart; + SequenceNumberSet gapList; + + static uint16_t getRawSizeWithoutSNSet() { + return SubmessageHeader::getRawSize() + (2 * (3 + 1) + 8); // 2*EntityID + GapStart + } + + static uint16_t getRawSizeWithSingleElementSNSet() { + return SubmessageHeader::getRawSize() + + (2 * (3 + 1) + 8 + 8 + 4); // 2*EntityID + GapStart + bitmapBase + numBits + } +}; + +struct SubmessageAckNack { + SubmessageHeader header; + EntityId_t readerId; + EntityId_t writerId; + SequenceNumberSet readerSNState; + Count_t count; + static uint16_t getRawSize(const SequenceNumberSet &set) { + uint16_t bitMapSize = 0; + if (set.numBits != 0) { + bitMapSize = static_cast(4 * (((set.numBits - 1) / 32) + 1)); + } + return getRawSizeWithoutSNSet() + sizeof(SequenceNumber_t) + sizeof(uint32_t) + + bitMapSize; // SequenceNumberSet + } + static uint16_t getRawSizeWithoutSNSet() { + return SubmessageHeader::getRawSize() + (2 * (3 + 1)) + sizeof(Count_t); + } +}; + +template bool serializeMessage(Buffer &buffer, Header &header) { + if (!buffer.reserve(Header::getRawSize())) { + return false; + } + + buffer.append(header.protocolName.data(), sizeof(std::array)); + buffer.append(reinterpret_cast(&header.protocolVersion), sizeof(ProtocolVersion_t)); + buffer.append(header.vendorId.vendorId.data(), sizeof(VendorId_t)); + buffer.append(header.guidPrefix.id.data(), sizeof(GuidPrefix_t)); + return true; +} + +template bool serializeMessage(Buffer &buffer, SubmessageHeader &header) { + if (!buffer.reserve(Header::getRawSize())) { + return false; + } + buffer.reserve(SubmessageHeader::getRawSize()); + + buffer.append(reinterpret_cast(&header.submessageId), sizeof(SubmessageKind)); + buffer.append(&header.flags, sizeof(uint8_t)); + buffer.append(reinterpret_cast(&header.octetsToNextHeader), sizeof(uint16_t)); + return true; +} + +template bool serializeMessage(Buffer &buffer, SubmessageInfoDST &msg) { + if (!buffer.reserve(SubmessageInfoDST::getRawSize())) { + return false; + } + + bool ret = serializeMessage(buffer, msg.header); + if (!ret) { + return false; + } + + return buffer.append(msg.guidPrefix.id.data(), sizeof(GuidPrefix_t)); +} + +template bool serializeMessage(Buffer &buffer, SubmessageData &msg) { + if (!buffer.reserve(SubmessageData::getRawSize())) { + return false; + } + + serializeMessage(buffer, msg.header); + + buffer.append(reinterpret_cast(&msg.extraFlags), sizeof(uint16_t)); + buffer.append(reinterpret_cast(&msg.octetsToInlineQos), sizeof(uint16_t)); + buffer.append(msg.readerId.entityKey.data(), msg.readerId.entityKey.size()); + buffer.append(reinterpret_cast(&msg.readerId.entityKind), sizeof(EntityKind_t)); + buffer.append(msg.writerId.entityKey.data(), msg.writerId.entityKey.size()); + buffer.append(reinterpret_cast(&msg.writerId.entityKind), sizeof(EntityKind_t)); + buffer.append(reinterpret_cast(&msg.writerSN.high), sizeof(msg.writerSN.high)); + buffer.append(reinterpret_cast(&msg.writerSN.low), sizeof(msg.writerSN.low)); + return true; +} + +template bool serializeMessage(Buffer &buffer, SubmessageHeartbeat &msg) { + if (!buffer.reserve(SubmessageHeartbeat::getRawSize())) { + return false; + } + + serializeMessage(buffer, msg.header); + + buffer.append(msg.readerId.entityKey.data(), msg.readerId.entityKey.size()); + buffer.append(reinterpret_cast(&msg.readerId.entityKind), sizeof(EntityKind_t)); + buffer.append(msg.writerId.entityKey.data(), msg.writerId.entityKey.size()); + buffer.append(reinterpret_cast(&msg.writerId.entityKind), sizeof(EntityKind_t)); + buffer.append(reinterpret_cast(&msg.firstSN.high), sizeof(msg.firstSN.high)); + buffer.append(reinterpret_cast(&msg.firstSN.low), sizeof(msg.firstSN.low)); + buffer.append(reinterpret_cast(&msg.lastSN.high), sizeof(msg.lastSN.high)); + buffer.append(reinterpret_cast(&msg.lastSN.low), sizeof(msg.lastSN.low)); + buffer.append(reinterpret_cast(&msg.count.value), sizeof(msg.count.value)); + return true; +} + +template bool serializeMessage(Buffer &buffer, SubmessageAckNack &msg) { + if (!buffer.reserve(SubmessageAckNack::getRawSize(msg.readerSNState))) { + return false; + } + + serializeMessage(buffer, msg.header); + + buffer.append(msg.readerId.entityKey.data(), msg.readerId.entityKey.size()); + buffer.append(reinterpret_cast(&msg.readerId.entityKind), sizeof(EntityKind_t)); + buffer.append(msg.writerId.entityKey.data(), msg.writerId.entityKey.size()); + buffer.append(reinterpret_cast(&msg.writerId.entityKind), sizeof(EntityKind_t)); + buffer.append(reinterpret_cast(&msg.readerSNState.base.high), + sizeof(msg.readerSNState.base.high)); + buffer.append(reinterpret_cast(&msg.readerSNState.base.low), + sizeof(msg.readerSNState.base.low)); + buffer.append(reinterpret_cast(&msg.readerSNState.numBits), sizeof(uint32_t)); + if (msg.readerSNState.numBits != 0) { + buffer.append(reinterpret_cast(msg.readerSNState.bitMap.data()), + 4 * (((msg.readerSNState.numBits - 1) / 32) + 1)); + } + buffer.append(reinterpret_cast(&msg.count.value), sizeof(msg.count.value)); + return true; +} + +template bool serializeMessage(Buffer &buffer, SubmessageGap &msg) { + if (msg.gapList.numBits != 0) { + return false; + } + if (!buffer.reserve(36)) { + return false; + } + + serializeMessage(buffer, msg.header); + + buffer.append(msg.readerId.entityKey.data(), msg.readerId.entityKey.size()); + buffer.append(reinterpret_cast(&msg.readerId.entityKind), sizeof(EntityKind_t)); + buffer.append(msg.writerId.entityKey.data(), msg.writerId.entityKey.size()); + buffer.append(reinterpret_cast(&msg.writerId.entityKind), sizeof(EntityKind_t)); + + buffer.append(reinterpret_cast(&msg.gapStart.high), sizeof(msg.gapStart.high)); + buffer.append(reinterpret_cast(&msg.gapStart.low), sizeof(msg.gapStart.low)); + + buffer.append(reinterpret_cast(&msg.gapList.base.high), sizeof(msg.gapList.base.high)); + buffer.append(reinterpret_cast(&msg.gapList.base.low), sizeof(msg.gapList.base.low)); + + buffer.append(reinterpret_cast(&msg.gapList.numBits), sizeof(uint32_t)); + + return true; +} + +struct MessageProcessingInfo { + MessageProcessingInfo(const uint8_t *data, DataSize_t size) + : data(data) + , size(size) {} + const uint8_t *data; + const DataSize_t size; + + //! Offset to the next unprocessed byte + DataSize_t nextPos = 0; + + inline const uint8_t *getPointerToCurrentPos() const { return &data[nextPos]; } + + //! Returns the size of data which isn't processed yet + inline DataSize_t getRemainingSize() const { return size - nextPos; } +}; + +bool deserializeMessage(const MessageProcessingInfo &info, Header &header); + +bool deserializeMessage(const MessageProcessingInfo &info, SubmessageHeader &header); + +bool deserializeMessage(const MessageProcessingInfo &info, SubmessageData &msg); + +bool deserializeMessage(const MessageProcessingInfo &info, SubmessageHeartbeat &msg); + +bool deserializeMessage(const MessageProcessingInfo &info, SubmessageAckNack &msg); + +bool deserializeMessage(const MessageProcessingInfo &info, SubmessageGap &msg); + +void deserializeSNS(const uint8_t *&position, SequenceNumberSet &set, std::size_t num_bitfields); + +} // namespace rtps + +#if defined(_MSC_VER) +#pragma pack(pop) +#endif +#undef RTPS_EMBEDDED_PACKED + +#endif // RTPS_MESSAGES_H diff --git a/components/rtps_embedded/include/rtps/rtps.hpp b/components/rtps_embedded/include/rtps/rtps.hpp new file mode 100644 index 000000000..6838075b2 --- /dev/null +++ b/components/rtps_embedded/include/rtps/rtps.hpp @@ -0,0 +1,33 @@ +/* +The MIT License +Copyright (c) 2019 Lehrstuhl Informatik 11 - RWTH Aachen University +Modifications Copyright (c) 2026 ATDev +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: +The above copyright notice and this permission notice shall be included in +all copies or substantial portions of the Software. +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +THE SOFTWARE + +This file is part of embeddedRTPS. + +Author: i11 - Embedded Software, RWTH Aachen University +*/ + +#ifndef RTPS_RTPS_H +#define RTPS_RTPS_H + +#include "rtps/entities/Domain.hpp" + +namespace rtps {} // namespace rtps + +#endif // RTPS_RTPS_H diff --git a/components/rtps_embedded/include/rtps/storages/CacheChange.hpp b/components/rtps_embedded/include/rtps/storages/CacheChange.hpp new file mode 100644 index 000000000..46bf8c64e --- /dev/null +++ b/components/rtps_embedded/include/rtps/storages/CacheChange.hpp @@ -0,0 +1,74 @@ +/* +The MIT License +Copyright (c) 2019 Lehrstuhl Informatik 11 - RWTH Aachen University +Modifications Copyright (c) 2026 ATDev +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: +The above copyright notice and this permission notice shall be included in +all copies or substantial portions of the Software. +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +THE SOFTWARE + +This file is part of embeddedRTPS. + +Author: i11 - Embedded Software, RWTH Aachen University +*/ + +#ifndef PROJECT_CACHECHANGE_H +#define PROJECT_CACHECHANGE_H + +#include "rtps/common/types.hpp" +#include "rtps/storages/PayloadBuffer.hpp" + +#include + +namespace rtps { +struct CacheChange { + using TimePoint = std::chrono::steady_clock::time_point; + + ChangeKind_t kind = ChangeKind_t::INVALID; + bool inLineQoS = false; + bool disposeAfterWrite = false; + TimePoint sentTime{}; + SequenceNumber_t sequenceNumber = SEQUENCENUMBER_UNKNOWN; + PayloadBuffer data; + + CacheChange &operator=(const CacheChange &other) = delete; + + CacheChange &operator=(CacheChange &&other) noexcept { + kind = other.kind; + inLineQoS = other.inLineQoS; + disposeAfterWrite = other.disposeAfterWrite; + sentTime = other.sentTime; + sequenceNumber = other.sequenceNumber; + data = std::move(other.data); + return *this; + } + + CacheChange() = default; + CacheChange(ChangeKind_t kind, SequenceNumber_t sequenceNumber) + : kind(kind) + , sequenceNumber(sequenceNumber){}; + + void reset() { + kind = ChangeKind_t::INVALID; + sequenceNumber = SEQUENCENUMBER_UNKNOWN; + inLineQoS = false; + disposeAfterWrite = false; + sentTime = TimePoint{}; + } + + bool isInitialized() const { return (kind != ChangeKind_t::INVALID); } +}; +} // namespace rtps + +#endif // PROJECT_CACHECHANGE_H diff --git a/components/rtps_embedded/include/rtps/storages/HistoryCacheWithDeletion.hpp b/components/rtps_embedded/include/rtps/storages/HistoryCacheWithDeletion.hpp new file mode 100644 index 000000000..7657a6b75 --- /dev/null +++ b/components/rtps_embedded/include/rtps/storages/HistoryCacheWithDeletion.hpp @@ -0,0 +1,276 @@ +/* +The MIT License +Copyright (c) 2019 Lehrstuhl Informatik 11 - RWTH Aachen University +Modifications Copyright (c) 2026 ATDev +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: +The above copyright notice and this permission notice shall be included in +all copies or substantial portions of the Software. +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +THE SOFTWARE + +This file is part of embeddedRTPS. + +Author: i11 - Embedded Software, RWTH Aachen University +*/ + +#ifndef HISTORYCACHEWITHDELETION_H +#define HISTORYCACHEWITHDELETION_H + +#include +#include + +namespace rtps { + +/** + * Extension of the SimpleHistoryCache that allows for deletion operation at the + * cost of efficieny + * TODO: Replace with something better in the future! + * Likely only used for SEDP + */ +template class HistoryCacheWithDeletion { +public: + HistoryCacheWithDeletion() = default; + + uint32_t m_dispose_after_write_cnt = 0; + + bool isFull() const { + uint16_t it = m_head; + incrementIterator(it); + return it == m_tail; + } + + const CacheChange *addChange(const uint8_t *data, DataSize_t size, bool inLineQoS, + bool disposeAfterWrite) { + CacheChange change; + change.kind = ChangeKind_t::ALIVE; + change.inLineQoS = inLineQoS; + change.disposeAfterWrite = disposeAfterWrite; + change.data.reserve(size); + change.data.append(data, size); + change.sequenceNumber = ++m_lastUsedSequenceNumber; + + if (disposeAfterWrite) { + m_dispose_after_write_cnt++; + } + + CacheChange *place = &m_buffer[m_head]; + incrementHead(); + + *place = std::move(change); + return place; + } + + const CacheChange *addChange(const uint8_t *data, DataSize_t size) { + return addChange(data, size, 0, false); + } + + void removeUntilIncl(SequenceNumber_t sn) { + if (m_head == m_tail) { + return; + } + + if (getCurrentSeqNumMax() <= sn) { // We won't overrun head + m_head = m_tail; + return; + } + + while (m_buffer[m_tail].sequenceNumber <= sn) { + incrementTail(); + } + } + + void dropOldest() { removeUntilIncl(getCurrentSeqNumMin()); } + + bool dropChange(const SequenceNumber_t &sn) { + uint16_t idx_to_clear; + CacheChange *change; + if (!getChangeBySN(sn, &change, idx_to_clear)) { + return false; // sn does not exist, nothing to do + } + + if (idx_to_clear == m_tail) { + m_buffer[m_tail].reset(); + incrementTail(); + return true; + } + + uint16_t prev = idx_to_clear; + do { + prev = idx_to_clear - 1; + if (prev >= m_buffer.size()) { + prev = m_buffer.size() - 1; + } + + m_buffer[idx_to_clear] = std::move(m_buffer[prev]); + idx_to_clear = prev; + + } while (prev != m_tail); + + incrementTail(); + + return true; + } + + bool setCacheChangeKind(const SequenceNumber_t &sn, ChangeKind_t kind) { + CacheChange *change = getChangeBySN(sn); + if (change == nullptr) { + return false; + } + + change->kind = kind; + return true; + } + + CacheChange *getChangeBySN(SequenceNumber_t sn) { + CacheChange *change; + uint16_t position; + if (getChangeBySN(sn, &change, position)) { + return change; + } else { + return nullptr; + } + } + + bool isEmpty() const { return (m_head == m_tail); } + + const SequenceNumber_t &getCurrentSeqNumMin() const { + if (m_head == m_tail) { + return SEQUENCENUMBER_UNKNOWN; + } else { + return m_buffer[m_tail].sequenceNumber; + } + } + + const SequenceNumber_t &getCurrentSeqNumMax() const { + if (m_head == m_tail) { + return SEQUENCENUMBER_UNKNOWN; + } else { + return m_lastUsedSequenceNumber; + } + } + + const SequenceNumber_t &getLastUsedSequenceNumber() const { return m_lastUsedSequenceNumber; } + + void clear() { + m_head = 0; + m_tail = 0; + m_lastUsedSequenceNumber = {0, 0}; + } +#ifdef DEBUG_HISTORY_CACHE_WITH_DELETION + void print() const { + for (unsigned int i = 0; i < m_buffer.size(); i++) { + std::cout << "[" << i << "] " + << " SN = " << m_buffer[i].sequenceNumber.low; + switch (m_buffer[i].kind) { + case ChangeKind_t::ALIVE: + std::cout << " Type = ALIVE"; + break; + case ChangeKind_t::INVALID: + std::cout << " Type = INVALID"; + break; + case ChangeKind_t::NOT_ALIVE_DISPOSED: + std::cout << " Type = DISPOSED"; + break; + } + if (m_head == i) { + std::cout << " <- HEAD"; + } + if (m_tail == i) { + std::cout << " <- TAIL"; + } + std::cout << std::endl; + } + } +#endif + bool isSNInRange(const SequenceNumber_t &sn) const { + if (isEmpty()) { + return false; + } + SequenceNumber_t minSN = getCurrentSeqNumMin(); + if (sn < minSN || getCurrentSeqNumMax() < sn) { + return false; + } + return true; + } + +private: + std::array m_buffer{}; + uint16_t m_head = 0; + uint16_t m_tail = 0; + static_assert(sizeof(SIZE) <= sizeof(m_head), "Iterator is large enough for given size"); + + SequenceNumber_t m_lastUsedSequenceNumber{0, 0}; + + bool getChangeBySN(const SequenceNumber_t &sn, CacheChange **out_change, + uint16_t &out_buffer_position) { + if (!isSNInRange(sn)) { + return false; + } + static_assert(std::is_unsigned::value, "Underflow well defined"); + static_assert(sizeof(m_tail) <= sizeof(uint16_t), "Cast ist well defined"); + + uint16_t cur_idx = m_tail; + while (cur_idx != m_head) { + if (m_buffer[cur_idx].sequenceNumber == sn) { + *out_change = &m_buffer[cur_idx]; + out_buffer_position = cur_idx; + return true; + } + // Sequence numbers are consecutive + if (m_buffer[cur_idx].sequenceNumber > sn) { + *out_change = nullptr; + return false; + } + + incrementIterator(cur_idx); + } + + *out_change = nullptr; + return false; + } + + inline void incrementHead() { + incrementIterator(m_head); + if (m_head == m_tail) { + // Move without check + incrementIterator(m_tail); // drop one + } + } + + inline void incrementIterator(uint16_t &iterator) const { + ++iterator; + if (iterator >= m_buffer.size()) { + iterator = 0; + } + } + + inline void incrementTail() { + if (m_buffer[m_tail].disposeAfterWrite) { + m_dispose_after_write_cnt--; + } + if (m_head != m_tail) { + m_buffer[m_tail].reset(); + incrementIterator(m_tail); + } + } + +protected: + // This constructor was created for unit testing + explicit HistoryCacheWithDeletion(SequenceNumber_t lastUsed) + : HistoryCacheWithDeletion() { + m_lastUsedSequenceNumber = lastUsed; + } +}; +} // namespace rtps + +#endif // HISTORYCACHEWITHDELETION_H diff --git a/components/rtps_embedded/include/rtps/storages/MemoryPool.hpp b/components/rtps_embedded/include/rtps/storages/MemoryPool.hpp new file mode 100644 index 000000000..b25fddedc --- /dev/null +++ b/components/rtps_embedded/include/rtps/storages/MemoryPool.hpp @@ -0,0 +1,188 @@ +/* +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]; } + + pointer operator->() const { return &m_pool.m_data[m_bit]; } + + // Pre-increment + MemoryPoolIterator &operator++() { + if (m_pool.m_numElements == 0) { + m_bit = SIZE; + return *this; + } + uint32_t bucket; + do { + ++m_bit; + bucket = m_bit / static_cast(8); + } while (!(m_bitMap[bucket] & (1 << (m_bit % 8))) && m_bit < SIZE); + + return *this; + } + + // Post-increment + MemoryPoolIterator operator++(int) { + MemoryPoolIterator tmp(*this); + ++(*this); + return tmp; + } + + private: + friend class MemoryPool; + MemoryPool &m_pool; + uint8_t m_bitMap[SIZE / 8 + 1]; + uint32_t m_bit = 0; + }; + + typedef MemoryPoolIterator MemPoolIter; + typedef MemoryPoolIterator const_MemPoolIter; + + typedef bool (*condition_fp)(TYPE); + + uint32_t getSize() { return SIZE; } + + bool isFull() const { return m_numElements == SIZE; } + + bool isEmpty() const { return m_numElements == 0; } + + uint32_t getNumElements() const { return m_numElements; } + + bool add(const TYPE &data) { + if (isFull()) { + printf("[MemoryPool] RESSOURCE LIMIT EXCEEDED \n"); + return false; + } + for (uint32_t bucket = 0; bucket < sizeof(m_bitMap); ++bucket) { + if (m_bitMap[bucket] != 0xFF) { + uint8_t byte = m_bitMap[bucket]; + for (uint8_t bit = 0; bit < 8; ++bit) { + if (!(byte & 1)) { + m_bitMap[bucket] |= 1 << bit; + m_data[bucket * 8 + bit] = data; + ++m_numElements; + return true; + } + byte = byte >> 1; + } + } + } + return false; + } + + /** + * Parameters are used in that way to allow lambdas with captures. Use this by + * creating two: E.g.: auto callback=[data](TYPE& value){return value == + * data;}; auto thunk=[](void* arg, TYPE& value){return + * (*static_cast(arg))(value);}; + * + * and then simply call: + * remove(thunk, &callback) + * + * NOTE: You have to make sure that the callback did not run out of scope. + */ + bool remove(bool (*jumppad)(void *, const TYPE &data), void *isCorrectElement) { + bool retcode = false; + for (auto it = begin(); it != end(); ++it) { + if (jumppad(isCorrectElement, *it)) { + const uint32_t bucket = it.m_bit / uint32_t{8}; + const uint32_t pos = + it.m_bit & uint32_t{7}; // 7 sets all bits above and including the one for 8 to 0 + m_bitMap[bucket] &= ~(static_cast(1) << pos); + --m_numElements; + retcode = true; + } + } + return retcode; + } + + void clear() { + for (unsigned int i = 0; i < (SIZE / 8 + 1); i++) { + m_bitMap[i] = 0; + } + m_numElements = 0; + } + + TYPE *find(bool (*jumppad)(void *, const TYPE &data), void *isCorrectElement) { + for (auto it = begin(); it != end(); ++it) { + if (jumppad(isCorrectElement, *it)) { + return &(*it); + } + } + return nullptr; + } + + MemPoolIter begin() { + MemPoolIter it(*this); + if (!(m_bitMap[0] & 1)) { + ++it; + } + return it; + } + + MemPoolIter end() { + MemPoolIter endIt(*this); + endIt.m_bit = SIZE; + return endIt; + } + +private: + uint8_t m_bitMap[SIZE / 8 + 1]{}; + uint32_t m_numElements = 0; + TYPE m_data[SIZE]; +}; + +} // namespace rtps + +#endif // RTPS_MEMORYPOOL_H diff --git a/components/rtps_embedded/include/rtps/storages/PayloadBuffer.hpp b/components/rtps_embedded/include/rtps/storages/PayloadBuffer.hpp new file mode 100644 index 000000000..0637837dc --- /dev/null +++ b/components/rtps_embedded/include/rtps/storages/PayloadBuffer.hpp @@ -0,0 +1,71 @@ +/* +The MIT License +Copyright (c) 2019 Lehrstuhl Informatik 11 - RWTH Aachen University +Modifications Copyright (c) 2026 ATDev +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: +The above copyright notice and this permission notice shall be included in +all copies or substantial portions of the Software. +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +THE SOFTWARE + +This file is part of embeddedRTPS. + +Author: i11 - Embedded Software, RWTH Aachen University +*/ + +#ifndef RTPS_PAYLOADBUFFER_H +#define RTPS_PAYLOADBUFFER_H + +#include + +#include "rtps/common/types.hpp" + +namespace rtps { + +struct PayloadBuffer { + std::vector bytes; + + bool isValid() const { return true; } + + bool reserve(DataSize_t length) { + if (length > bytes.max_size() - bytes.size()) { + return false; + } + bytes.reserve(bytes.size() + length); + return true; + } + + bool append(const uint8_t *data, DataSize_t length) { + if (length == 0) { + return true; + } + if (data == nullptr) { + return false; + } + + bytes.insert(bytes.end(), data, data + length); + return true; + } + + void append(const PayloadBuffer &other) { + bytes.insert(bytes.end(), other.bytes.begin(), other.bytes.end()); + } + + DataSize_t spaceUsed() const { return static_cast(bytes.size()); } + + void reset() { bytes.clear(); } +}; + +} // namespace rtps + +#endif // RTPS_PAYLOADBUFFER_H diff --git a/components/rtps_embedded/include/rtps/storages/SimpleHistoryCache.hpp b/components/rtps_embedded/include/rtps/storages/SimpleHistoryCache.hpp new file mode 100644 index 000000000..540560dbc --- /dev/null +++ b/components/rtps_embedded/include/rtps/storages/SimpleHistoryCache.hpp @@ -0,0 +1,177 @@ +/* +The MIT License +Copyright (c) 2019 Lehrstuhl Informatik 11 - RWTH Aachen University +Modifications Copyright (c) 2026 ATDev +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: +The above copyright notice and this permission notice shall be included in +all copies or substantial portions of the Software. +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +THE SOFTWARE + +This file is part of embeddedRTPS. + +Author: i11 - Embedded Software, RWTH Aachen University +*/ + +#ifndef PROJECT_SIMPLEHISTORYCACHE_H +#define PROJECT_SIMPLEHISTORYCACHE_H + +#include "rtps/config.hpp" +#include "rtps/storages/CacheChange.hpp" + +namespace rtps { + +/** + * Simple version of a history cache. It sets consecutive sequence numbers + * automatically which allows an easy and fast approach of dropping acknowledged + * changes. Furthermore, disposing of arbitrary changes is not possible. + * However, this is in principle easy to add by changing the ChangeKind and + * dropping it when passing it during deleting of other sequence numbers + */ +template class SimpleHistoryCache { +public: + SimpleHistoryCache() = default; + + bool isFull() const { + uint16_t it = m_head; + incrementIterator(it); + return it == m_tail; + } + + const CacheChange *addChange(const uint8_t *data, DataSize_t size, bool inLineQoS, + bool disposeAfterWrite) { + CacheChange change; + change.kind = ChangeKind_t::ALIVE; + change.inLineQoS = inLineQoS; + change.disposeAfterWrite = disposeAfterWrite; + change.data.reserve(size); + change.data.append(data, size); + change.sequenceNumber = ++m_lastUsedSequenceNumber; + + CacheChange *place = &m_buffer[m_head]; + incrementHead(); + + *place = std::move(change); + return place; + } + + const CacheChange *addChange(const uint8_t *data, DataSize_t size) { + return addChange(data, size, 0, false); + } + + void removeUntilIncl(SequenceNumber_t sn) { + if (m_head == m_tail) { + return; + } + + if (getSeqNumMax() <= sn) { // We won't overrun head + m_head = m_tail; + return; + } + + while (m_buffer[m_tail].sequenceNumber <= sn && (m_head != m_tail)) { + incrementTail(); + } + } + + void dropOldest() { removeUntilIncl(getSeqNumMin()); } + + bool setCacheChangeKind(const SequenceNumber_t &sn, ChangeKind_t kind) { + CacheChange *change = getChangeBySN(sn); + if (change == nullptr) { + return false; + } + + change->kind = kind; + return true; + } + + CacheChange *getChangeBySN(SequenceNumber_t sn) { + SequenceNumber_t minSN = getSeqNumMin(); + if (sn < minSN || getSeqNumMax() < sn) { + return nullptr; + } + static_assert(std::is_unsigned::value, "Underflow well defined"); + static_assert(sizeof(m_tail) <= sizeof(uint16_t), "Cast ist well defined"); + // We don't overtake head, therefore difference of sn is within same range + // as iterators + uint16_t pos = m_tail + static_cast(sn.low - minSN.low); + + // Diff is smaller than the size of the array -> max one overflow + if (pos >= m_buffer.size()) { + pos -= m_buffer.size(); + } + return &m_buffer[pos]; + } + + const SequenceNumber_t &getSeqNumMin() const { + if (m_head == m_tail) { + return SEQUENCENUMBER_UNKNOWN; + } else { + return m_buffer[m_tail].sequenceNumber; + } + } + + const SequenceNumber_t &getSeqNumMax() const { + if (m_head == m_tail) { + return SEQUENCENUMBER_UNKNOWN; + } else { + return m_lastUsedSequenceNumber; + } + } + + void clear() { + m_head = 0; + m_tail = 0; + m_lastUsedSequenceNumber = {0, 0}; + } + +private: + std::array m_buffer{}; + uint16_t m_head = 0; + uint16_t m_tail = 0; + static_assert(sizeof(SIZE) <= sizeof(m_head), "Iterator is large enough for given size"); + + SequenceNumber_t m_lastUsedSequenceNumber{0, 0}; + + inline void incrementHead() { + incrementIterator(m_head); + if (m_head == m_tail) { + // Move without check + incrementIterator(m_tail); // drop one + } + } + + inline void incrementIterator(uint16_t &iterator) const { + ++iterator; + if (iterator >= m_buffer.size()) { + iterator = 0; + } + } + + inline void incrementTail() { + if (m_head != m_tail) { + incrementIterator(m_tail); + } + } + +protected: + // This constructor was created for unit testing + explicit SimpleHistoryCache(SequenceNumber_t lastUsed) + : SimpleHistoryCache() { + m_lastUsedSequenceNumber = lastUsed; + } +}; +} // namespace rtps + +#endif // PROJECT_SIMPLEHISTORYCACHE_H diff --git a/components/rtps_embedded/include/rtps/storages/ThreadSafeCircularBuffer.hpp b/components/rtps_embedded/include/rtps/storages/ThreadSafeCircularBuffer.hpp new file mode 100644 index 000000000..fb0b95f15 --- /dev/null +++ b/components/rtps_embedded/include/rtps/storages/ThreadSafeCircularBuffer.hpp @@ -0,0 +1,76 @@ +/* +The MIT License +Copyright (c) 2019 Lehrstuhl Informatik 11 - RWTH Aachen University +Modifications Copyright (c) 2026 ATDev +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: +The above copyright notice and this permission notice shall be included in +all copies or substantial portions of the Software. +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +THE SOFTWARE + +This file is part of embeddedRTPS. + +Author: i11 - Embedded Software, RWTH Aachen University +*/ + +#ifndef RTPS_THREADSAFEQUEUE_H +#define RTPS_THREADSAFEQUEUE_H + +#include +#include +#include +#include + +namespace rtps { + +template class ThreadSafeCircularBuffer { + +public: + bool moveElementIntoBuffer(T &&elem); + bool copyElementIntoBuffer(const T &elem); + + /** + * Removes the first into the given hull. Also moves responsibility for + * resources. + * @return true if element was injected. False if no element was present. + */ + bool moveFirstInto(T &hull); + bool peakFirst(T &hull); + + uint32_t numElements(); + uint32_t insertionFailures(); + + void clear(); + +private: + std::array m_buffer{}; + uint16_t m_head = 0; + uint16_t m_tail = 0; + uint32_t m_num_elements = 0; + uint32_t m_insertion_failures = 0; + static_assert(SIZE + 1 < std::numeric_limits::max(), + "Iterator is large enough for given size"); + + std::mutex m_mutex; + + inline bool isFull() const; + inline void incrementIterator(uint16_t &iterator) const; + inline void incrementTail(); + inline void incrementHead(); +}; + +} // namespace rtps + +#include "ThreadSafeCircularBuffer.tpp" + +#endif // RTPS_THREADSAFEQUEUE_H diff --git a/components/rtps_embedded/include/rtps/storages/ThreadSafeCircularBuffer.tpp b/components/rtps_embedded/include/rtps/storages/ThreadSafeCircularBuffer.tpp new file mode 100644 index 000000000..35c1bd913 --- /dev/null +++ b/components/rtps_embedded/include/rtps/storages/ThreadSafeCircularBuffer.tpp @@ -0,0 +1,102 @@ + +#ifndef RTPS_THREADSAFECIRCULARBUFFER_TPP +#define RTPS_THREADSAFECIRCULARBUFFER_TPP + +namespace rtps { + +template +bool ThreadSafeCircularBuffer::moveElementIntoBuffer(T &&elem) { + std::lock_guard lock(m_mutex); + if (!isFull()) { + m_buffer[m_head] = std::move(elem); + incrementHead(); + return true; + } else { + m_insertion_failures++; + return false; + } +} + +template +bool ThreadSafeCircularBuffer::copyElementIntoBuffer(const T &elem) { + std::lock_guard lock(m_mutex); + if (!isFull()) { + m_buffer[m_head] = elem; + incrementHead(); + return true; + } else { + m_insertion_failures++; + return false; + } +} + +template +bool ThreadSafeCircularBuffer::moveFirstInto(T &hull) { + std::lock_guard lock(m_mutex); + if (m_head != m_tail) { + hull = std::move(m_buffer[m_tail]); + incrementTail(); + return true; + } else { + return false; + } +} + +template bool ThreadSafeCircularBuffer::peakFirst(T &hull) { + std::lock_guard lock(m_mutex); + if (m_head != m_tail) { + hull = m_buffer[m_tail]; + return true; + } else { + return false; + } +} + +template uint32_t ThreadSafeCircularBuffer::numElements() { + std::lock_guard lock(m_mutex); + return m_num_elements; +} + +template +uint32_t ThreadSafeCircularBuffer::insertionFailures() { + std::lock_guard lock(m_mutex); + return m_insertion_failures; +} + +template void ThreadSafeCircularBuffer::clear() { + std::lock_guard lock(m_mutex); + m_head = m_tail; + m_num_elements = 0; +} + +template bool ThreadSafeCircularBuffer::isFull() const { + auto it = m_head; + incrementIterator(it); + return it == m_tail; +} + +template +inline void ThreadSafeCircularBuffer::incrementIterator(uint16_t &iterator) const { + ++iterator; + if (iterator >= m_buffer.size()) { + iterator = 0; + } +} + +template +inline void ThreadSafeCircularBuffer::incrementTail() { + incrementIterator(m_tail); + m_num_elements--; +} + +template +inline void ThreadSafeCircularBuffer::incrementHead() { + incrementIterator(m_head); + m_num_elements++; + if (m_head == m_tail) { + incrementTail(); + } +} +} // namespace rtps + +#endif // RTPS_THREADSAFECIRCULARBUFFER_TPP diff --git a/components/rtps_embedded/include/rtps/utils/Diagnostics.hpp b/components/rtps_embedded/include/rtps/utils/Diagnostics.hpp new file mode 100644 index 000000000..1cc9e9550 --- /dev/null +++ b/components/rtps_embedded/include/rtps/utils/Diagnostics.hpp @@ -0,0 +1,86 @@ +/* +The MIT License +Copyright (c) 2019 Lehrstuhl Informatik 11 - RWTH Aachen University +Modifications Copyright (c) 2026 ATDev +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: +The above copyright notice and this permission notice shall be included in +all copies or substantial portions of the Software. +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +THE SOFTWARE + +This file is part of embeddedRTPS. + +Author: i11 - Embedded Software, RWTH Aachen University +*/ + +#ifndef RTPS_DIAGNOSTICS_H +#define RTPS_DIAGNOSTICS_H + +#include + +namespace rtps { +namespace Diagnostics { + +namespace ThreadPool { +extern uint32_t dropped_incoming_packets_usertraffic; +extern uint32_t dropped_incoming_packets_metatraffic; + +extern uint32_t dropped_outgoing_packets_usertraffic; +extern uint32_t dropped_outgoing_packets_metatraffic; + +extern uint32_t processed_incoming_metatraffic; +extern uint32_t processed_outgoing_metatraffic; +extern uint32_t processed_incoming_usertraffic; +extern uint32_t processed_outgoing_usertraffic; + +extern uint32_t max_ever_elements_outgoing_usertraffic_queue; +extern uint32_t max_ever_elements_incoming_usertraffic_queue; + +extern uint32_t max_ever_elements_outgoing_metatraffic_queue; +extern uint32_t max_ever_elements_incoming_metatraffic_queue; +} // namespace ThreadPool + +namespace StatefulReader { +extern uint32_t sfr_unexpected_sn; +extern uint32_t sfr_retransmit_requests; +} // namespace StatefulReader + +namespace Network { +extern uint32_t lwip_allocation_failures; +} + +namespace OS { +extern uint32_t current_free_heap_size; +} + +namespace SEDP { +extern uint32_t max_ever_remote_participants; +extern uint32_t current_remote_participants; + +extern uint32_t max_ever_matched_reader_proxies; +extern uint32_t current_max_matched_reader_proxies; + +extern uint32_t max_ever_matched_writer_proxies; +extern uint32_t current_max_matched_writer_proxies; + +extern uint32_t max_ever_unmatched_reader_proxies; +extern uint32_t current_max_unmatched_reader_proxies; + +extern uint32_t max_ever_unmatched_writer_proxies; +extern uint32_t current_max_unmatched_writer_proxies; +} // namespace SEDP + +} // namespace Diagnostics +} // namespace rtps + +#endif diff --git a/components/rtps_embedded/include/rtps/utils/Log.hpp b/components/rtps_embedded/include/rtps/utils/Log.hpp new file mode 100644 index 000000000..ffcb35f0f --- /dev/null +++ b/components/rtps_embedded/include/rtps/utils/Log.hpp @@ -0,0 +1,48 @@ +/* +The MIT License +Copyright (c) 2019 Lehrstuhl Informatik 11 - RWTH Aachen University +Modifications Copyright (c) 2026 ATDev +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: +The above copyright notice and this permission notice shall be included in +all copies or substantial portions of the Software. +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +THE SOFTWARE + +This file is part of embeddedRTPS. + +Author: i11 - Embedded Software, RWTH Aachen University +*/ + +#ifndef RTPS_LOG_H +#define RTPS_LOG_H + +#include +#include + +#define RTPS_GLOBAL_VERBOSE 0 + +#define SFW_VERBOSE 1 +#define SPDP_VERBOSE 0 +#define PBUF_WRAP_VERBOSE 0 +#define SEDP_VERBOSE 1 +#define RECV_VERBOSE 1 +#define PARTICIPANT_VERBOSE 0 +#define DOMAIN_VERBOSE 1 +#define UDP_DRIVER_VERBOSE 1 +#define TSCB_VERBOSE 1 +#define SLW_VERBOSE 0 +#define SFR_VERBOSE 1 +#define SLR_VERBOSE 1 +#define THREAD_POOL_VERBOSE 1 + +#endif // RTPS_LOG_H diff --git a/components/rtps_embedded/include/rtps/utils/constants.hpp b/components/rtps_embedded/include/rtps/utils/constants.hpp new file mode 100644 index 000000000..58b5c7c74 --- /dev/null +++ b/components/rtps_embedded/include/rtps/utils/constants.hpp @@ -0,0 +1,29 @@ +/* +The MIT License +Copyright (c) 2019 Lehrstuhl Informatik 11 - RWTH Aachen University +Modifications Copyright (c) 2026 ATDev +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: +The above copyright notice and this permission notice shall be included in +all copies or substantial portions of the Software. +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +THE SOFTWARE + +This file is part of embeddedRTPS. + +Author: i11 - Embedded Software, RWTH Aachen University +*/ + +#ifndef RTPS_CONSTANTS_H +#define RTPS_CONSTANTS_H + +#endif // RTPS_CONSTANTS_H diff --git a/components/rtps_embedded/include/rtps/utils/hash.hpp b/components/rtps_embedded/include/rtps/utils/hash.hpp new file mode 100644 index 000000000..2cd6ae5e6 --- /dev/null +++ b/components/rtps_embedded/include/rtps/utils/hash.hpp @@ -0,0 +1,42 @@ +/* +The MIT License +Copyright (c) 2019 Lehrstuhl Informatik 11 - RWTH Aachen University +Modifications Copyright (c) 2026 ATDev +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: +The above copyright notice and this permission notice shall be included in +all copies or substantial portions of the Software. +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +THE SOFTWARE + +This file is part of embeddedRTPS. + +Author: i11 - Embedded Software, RWTH Aachen University +*/ + +#ifndef RTPS_HASH_H +#define RTPS_HASH_H + +#include + +namespace rtps { +inline size_t hashCharArray(const char *p, size_t s) { + size_t result = 0; + const size_t prime = 31; + for (size_t i = 0; i < s; ++i) { + result = p[i] + (result * prime); + } + return result; +} +} // namespace rtps + +#endif diff --git a/components/rtps_embedded/include/rtps/utils/printutils.hpp b/components/rtps_embedded/include/rtps/utils/printutils.hpp new file mode 100644 index 000000000..eae1a2207 --- /dev/null +++ b/components/rtps_embedded/include/rtps/utils/printutils.hpp @@ -0,0 +1,30 @@ +// +// Created by andreas on 13.01.19. +// + +#ifndef RTPS_PRINTUTILS_H +#define RTPS_PRINTUTILS_H + +#include "rtps/common/types.hpp" + +inline void printEntityId(rtps::EntityId_t id) { + for (const auto byte : id.entityKey) { + printf("%x", (int)byte); + } + printf("%x", static_cast(id.entityKind)); + printf("\n"); +} + +inline void printGuidPrefix(rtps::GuidPrefix_t prefix) { + for (const auto byte : prefix.id) { + printf("%x", (int)byte); + } +} + +inline void printGuid(rtps::Guid_t guid) { + printGuidPrefix(guid.prefix); + printf(":"); + printEntityId(guid.entityId); +} + +#endif // RTPS_PRINTUTILS_H diff --git a/components/rtps_embedded/include/rtps/utils/sysFunctions.hpp b/components/rtps_embedded/include/rtps/utils/sysFunctions.hpp new file mode 100644 index 000000000..9801a539b --- /dev/null +++ b/components/rtps_embedded/include/rtps/utils/sysFunctions.hpp @@ -0,0 +1,46 @@ +/* +The MIT License +Copyright (c) 2019 Lehrstuhl Informatik 11 - RWTH Aachen University +Modifications Copyright (c) 2026 ATDev +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: +The above copyright notice and this permission notice shall be included in +all copies or substantial portions of the Software. +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +THE SOFTWARE + +This file is part of embeddedRTPS. + +Author: i11 - Embedded Software, RWTH Aachen University +*/ + +#ifndef PROJECT_SYSFUNCTIONS_H +#define PROJECT_SYSFUNCTIONS_H + +#include "rtps/common/types.hpp" + +#include + +namespace rtps { +inline Time_t getCurrentTimeStamp() { + static const auto start = std::chrono::steady_clock::now(); + const auto elapsed = std::chrono::steady_clock::now() - start; + const auto nowMs = std::chrono::duration_cast(elapsed).count(); + + Time_t now; + now.seconds = static_cast(nowMs / 1000); + now.fraction = static_cast((nowMs % 1000) * ((1ULL << 32) / 1000)); + return now; +} +} // namespace rtps + +#endif // PROJECT_SYSFUNCTIONS_H diff --git a/components/rtps_embedded/include/rtps/utils/udpUtils.hpp b/components/rtps_embedded/include/rtps/utils/udpUtils.hpp new file mode 100644 index 000000000..aed64ed09 --- /dev/null +++ b/components/rtps_embedded/include/rtps/utils/udpUtils.hpp @@ -0,0 +1,114 @@ +/* +The MIT License +Copyright (c) 2019 Lehrstuhl Informatik 11 - RWTH Aachen University +Modifications Copyright (c) 2026 ATDev +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: +The above copyright notice and this permission notice shall be included in +all copies or substantial portions of the Software. +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +THE SOFTWARE + +This file is part of embeddedRTPS. + +Author: i11 - Embedded Software, RWTH Aachen University +*/ + +#ifndef RTPS_UDP_UTILS_H +#define RTPS_UDP_UTILS_H + +#include "rtps/config.hpp" + +#include + +namespace rtps { +namespace { +const uint16_t PB = 7400; // Port Base Number +const uint16_t DG = 250; // DomainId Gain +const uint16_t PG = 2; // ParticipantId Gain +// Additional Offsets +const uint16_t D0 = 0; // Builtin multicast +const uint16_t D1 = 10; // Builtin unicast +const uint16_t D2 = 1; // User multicast +const uint16_t D3 = 11; // User unicast +} // namespace + +constexpr std::array transformIP4ToU32(uint8_t MSB, uint8_t p2, uint8_t p1, + uint8_t LSB) { + return {MSB, p2, p1, LSB}; +} + +constexpr Ip4Port_t getBuiltInUnicastPort(ParticipantId_t participantId) { + return PB + DG * Config::DOMAIN_ID + D1 + PG * participantId; +} + +constexpr Ip4Port_t getBuiltInMulticastPort() { return PB + DG * Config::DOMAIN_ID + D0; } + +constexpr Ip4Port_t getUserUnicastPort(ParticipantId_t participantId) { + return PB + DG * Config::DOMAIN_ID + D3 + PG * participantId; +} + +constexpr Ip4Port_t getUserMulticastPort() { return PB + DG * Config::DOMAIN_ID + D2; } + +constexpr bool isUserPort(Ip4Port_t port) { + return (port & 1) == 1; +} // really useful? There may be other user ports than just odd ones? + +inline bool isMultiCastPort(Ip4Port_t port) { + const auto idWithoutBase = port - PB - DG * Config::DOMAIN_ID; + return idWithoutBase == D0 || + (idWithoutBase >= D2 && idWithoutBase < D3); // There are several UserMulticastPorts! +} + +inline bool isMetaMultiCastPort(Ip4Port_t port) { + const auto idWithoutBase = port - PB - DG * Config::DOMAIN_ID; + return idWithoutBase == D0; +} + +inline bool isUserMultiCastPort(Ip4Port_t port) { + const auto idWithoutBase = port - PB - DG * Config::DOMAIN_ID; + return (idWithoutBase >= D2 && idWithoutBase < D1); +} + +inline bool isZeroAddress(const std::array &address) { + return address[0] == 0 && address[1] == 0 && address[2] == 0 && address[3] == 0; +} + +inline bool isMulticastAddress(const std::array &address) { + return address[0] >= 224 && address[0] <= 239; +} + +inline ParticipantId_t getParticipantIdFromUnicastPort(Ip4Port_t port, bool isUserPort) { + + const auto basePart = PB + DG * Config::DOMAIN_ID; + ParticipantId_t participantPart = port - basePart; + + uint16_t offset = 0; + if (isUserPort) { + offset = D3; + } else { + offset = D1; + } + + participantPart -= offset; + + auto id = static_cast(participantPart / PG); + if (id * PG + basePart + offset == port) { + return id; + } else { + return PARTICIPANT_ID_INVALID; + } +} + +} // namespace rtps + +#endif // RTPS_UDP_H diff --git a/components/rtps_embedded/src/ThreadPool.cpp b/components/rtps_embedded/src/ThreadPool.cpp new file mode 100644 index 000000000..1a33a0eae --- /dev/null +++ b/components/rtps_embedded/src/ThreadPool.cpp @@ -0,0 +1,305 @@ +/* +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.hpp" + +#include "rtps/entities/Domain.hpp" +#include "rtps/entities/Writer.hpp" +#include "rtps/utils/Diagnostics.hpp" +#include "rtps/utils/Log.hpp" +#include "rtps/utils/udpUtils.hpp" +#include "thread_pool.hpp" + +using rtps::ThreadPool; + +#if THREAD_POOL_VERBOSE && RTPS_GLOBAL_VERBOSE +#include "rtps/utils/printutils.hpp" +#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) { + + 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..95a43ce27 --- /dev/null +++ b/components/rtps_embedded/src/communication/EsppTransport.cpp @@ -0,0 +1,247 @@ +/* +The MIT License +Copyright (c) 2019 Lehrstuhl Informatik 11 - RWTH Aachen University +Modifications Copyright (c) 2026 ATDev +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: +The above copyright notice and this permission notice shall be included in +all copies or substantial portions of the Software. +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +THE SOFTWARE + +This file is part of embeddedRTPS. + +Author: i11 - Embedded Software, RWTH Aachen University +*/ + +#include "rtps/communication/EsppTransport.hpp" + +#include "task.hpp" + +#include +#include +#include +#include +#include + +using rtps::EsppTransport; + +namespace { + +bool parseIp4Address(const std::string &address, rtps::Ip4AddressBytes &out) { + rtps::Ip4AddressBytes parsed{0, 0, 0, 0}; + const char *cursor = address.c_str(); + + for (std::size_t i = 0; i < parsed.size(); ++i) { + char *end = nullptr; + const unsigned long value = std::strtoul(cursor, &end, 10); + if (end == cursor || value > 255) { + return false; + } + + parsed[i] = static_cast(value); + if (i + 1 < parsed.size()) { + if (*end != '.') { + return false; + } + cursor = end + 1; + } else if (*end != '\0') { + return false; + } + } + + out = parsed; + return true; +} + +bool isMulticastAddress(const rtps::Ip4AddressBytes &addr) { + return addr[0] >= 224 && addr[0] <= 239; +} + +} // namespace + +EsppTransport::EsppTransport(RxCallback callback, void *args) + : espp::BaseComponent("RtpsTransport", espp::Logger::Verbosity::WARN) + , m_rxCallback(callback) + , m_callbackArgs(args) {} + +EsppTransport::Channel *EsppTransport::findChannel(Ip4Port_t port) { + auto it = std::find_if(m_channels.begin(), m_channels.end(), [port](const auto &channel) { + return channel.in_use && channel.port == port; + }); + return (it != m_channels.end()) ? &(*it) : nullptr; +} + +const EsppTransport::Channel *EsppTransport::findChannel(Ip4Port_t port) const { + auto it = std::find_if(m_channels.begin(), m_channels.end(), [port](const auto &channel) { + return channel.in_use && channel.port == port; + }); + return (it != m_channels.end()) ? &(*it) : nullptr; +} + +std::string EsppTransport::ip4ToString(const Ip4AddressBytes &addr) { + return std::to_string(addr[0]) + "." + std::to_string(addr[1]) + "." + std::to_string(addr[2]) + + "." + std::to_string(addr[3]); +} + +bool EsppTransport::startReceiver(Channel &channel, Ip4Port_t receivePort) { + if (!channel.socket) { + logger_.error("startReceiver called with null socket on port {}", receivePort); + return false; + } + + 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::WARN; + channel.socket = std::make_unique(socket_config); + if (!channel.socket->is_valid()) { + logger_.error("Failed to create valid UDP socket for port {}", receivePort); + channel.socket.reset(); + return nullptr; + } + + 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); + const bool already_joined = + std::any_of(m_multicastGroups.begin(), m_multicastGroups.end(), + [&](const auto &existing_group) { return existing_group == group; }); + if (already_joined) { + return true; + } + + bool any_joined = false; + bool has_active_channels = false; + for (auto &channel : m_channels) { + if (!channel.in_use || !channel.socket) { + continue; + } + has_active_channels = true; + any_joined = channel.socket->add_multicast_group(group) || any_joined; + } + + if (!has_active_channels) { + // No active sockets yet: defer join until channels are created. + m_multicastGroups.push_back(group); + return true; + } + + if (any_joined) { + m_multicastGroups.push_back(group); + return true; + } + + logger_.warn("Failed to join multicast group {} on all active channels", group); + return false; +} + +void EsppTransport::sendPacket(PacketInfo &info) { + std::lock_guard lock(m_mutex); + + Channel *channel = findChannel(info.srcPort); + if (channel == nullptr) { + 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..bdbc9d24b --- /dev/null +++ b/components/rtps_embedded/src/discovery/ParticipantProxyData.cpp @@ -0,0 +1,259 @@ +/* +The MIT License +Copyright (c) 2019 Lehrstuhl Informatik 11 - RWTH Aachen University +Modifications Copyright (c) 2026 ATDev +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: +The above copyright notice and this permission notice shall be included in +all copies or substantial portions of the Software. +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +THE SOFTWARE + +This file is part of embeddedRTPS. + +Author: i11 - Embedded Software, RWTH Aachen University +*/ + +#include "rtps/discovery/ParticipantProxyData.hpp" +#include "rtps/entities/Participant.hpp" +#include "rtps/utils/Log.hpp" + +using rtps::ParticipantProxyData; + +#if SPDP_VERBOSE && RTPS_GLOBAL_VERBOSE +#define PPD_LOG(...) logger_.warn(__VA_ARGS__) +#else +#define PPD_LOG(...) \ + do { \ + } while (0) +#endif + +ParticipantProxyData::ParticipantProxyData(Guid_t guid) + : espp::BaseComponent("RtpsParticipantProxy", espp::Logger::Verbosity::WARN) + , m_guid(guid) {} + +void ParticipantProxyData::reset() { + m_guid = Guid_t{GUIDPREFIX_UNKNOWN, ENTITYID_UNKNOWN}; + m_manualLivelinessCount = Count_t{1}; + m_expectsInlineQos = false; + onAliveSignal(); + for (int i = 0; i < Config::SPDP_MAX_NUM_LOCATORS; ++i) { + m_metatrafficUnicastLocatorList[i].setInvalid(); + m_metatrafficMulticastLocatorList[i].setInvalid(); + m_defaultUnicastLocatorList[i].setInvalid(); + m_defaultMulticastLocatorList[i].setInvalid(); + } +} + +bool ParticipantProxyData::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) { + if (ucdr_buffer_remaining(&buffer) < sizeof(FullLengthLocator)) { + PPD_LOG("Not enough data left in buffer to read locator"); + return false; + } + ucdr_advance_buffer(&buffer, sizeof(FullLengthLocator)); + PPD_LOG("Max number of valid locators exceeded, ignoring this locator as we have at least " + "one valid locator"); + return true; + } + } + } + return false; +} + +#undef PPD_LOG diff --git a/components/rtps_embedded/src/discovery/SEDPAgent.cpp b/components/rtps_embedded/src/discovery/SEDPAgent.cpp new file mode 100644 index 000000000..1a098573d --- /dev/null +++ b/components/rtps_embedded/src/discovery/SEDPAgent.cpp @@ -0,0 +1,484 @@ +/* +The MIT License +Copyright (c) 2019 Lehrstuhl Informatik 11 - RWTH Aachen University +Modifications Copyright (c) 2026 ATDev +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: +The above copyright notice and this permission notice shall be included in +all copies or substantial portions of the Software. +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +THE SOFTWARE + +This file is part of embeddedRTPS. + +Author: i11 - Embedded Software, RWTH Aachen University +*/ + +#include "rtps/discovery/SEDPAgent.hpp" +#include "rtps/discovery/TopicData.hpp" +#include "rtps/entities/Participant.hpp" +#include "rtps/entities/Reader.hpp" +#include "rtps/entities/Writer.hpp" +#include "rtps/messages/MessageTypes.hpp" +#include "rtps/utils/Log.hpp" +#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, change.getDataSize()); + + 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, LocatorIPv4(writerData.unicastLocator), + (writerData.reliabilityKind == ReliabilityKind_t::RELIABLE)}); + if (mfp_onNewPublisherCallback != nullptr) { + mfp_onNewPublisherCallback(m_onNewPublisherArgs); + } +} + +void SEDPAgent::handleSubscriptionReaderMessage(const ReaderCacheChange &change) { + std::lock_guard lock(m_mutex); +#if SEDP_VERBOSE + SEDP_LOG("New subscriber"); +#endif + + if (!change.copyInto(m_buffer, sizeof(m_buffer) / sizeof(m_buffer[0]))) { +#if SEDP_VERBOSE + SEDP_LOG("SEDPAgent: Buffer too small."); +#endif + return; + } + ucdrBuffer cdrBuffer; + ucdr_init_buffer(&cdrBuffer, m_buffer, change.getDataSize()); + + 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, LocatorIPv4(readerData.unicastLocator), + LocatorIPv4(readerData.multicastLocator), + (readerData.reliabilityKind == ReliabilityKind_t::RELIABLE)}); + } else { + writer->addNewMatchedReader( + ReaderProxy{readerData.endpointGuid, LocatorIPv4(readerData.unicastLocator), + (readerData.reliabilityKind == ReliabilityKind_t::RELIABLE)}); + } + + if (mfp_onNewSubscriberCallback != nullptr) { + mfp_onNewSubscriberCallback(m_onNewSubscriberArgs); + } +} + +void SEDPAgent::tryMatchUnmatchedEndpoints() { + // Try to match remote readers with local writers + for (auto &proxy : m_unmatchedRemoteReaders) { + auto writer = m_part->getMatchingWriter(proxy); + if (writer != nullptr) { + writer->addNewMatchedReader(ReaderProxy{proxy.endpointGuid, proxy.unicastLocator, + proxy.multicastLocator, proxy.is_reliable}); + removeUnmatchedEntity(proxy.endpointGuid); + } + } + + // Try to match remote writers with local readers + for (auto &proxy : m_unmatchedRemoteWriters) { + auto reader = m_part->getMatchingReader(proxy); + if (reader != nullptr) { + reader->addNewMatchedWriter( + WriterProxy{proxy.endpointGuid, proxy.unicastLocator, proxy.is_reliable}); + removeUnmatchedEntity(proxy.endpointGuid); + } + } +} + +bool SEDPAgent::addWriter(Writer &writer) { + if (m_endpoints.sedpPubWriter == nullptr) { + return true; + } + EntityKind_t writerKind = writer.m_attributes.endpointGuid.entityId.entityKind; + if (writerKind == EntityKind_t::BUILD_IN_WRITER_WITH_KEY || + writerKind == EntityKind_t::BUILD_IN_WRITER_WITHOUT_KEY) { + return true; // No need to announce builtin endpoints + } + + std::lock_guard lock(m_mutex); + + // Check unmatched writers for this new reader + tryMatchUnmatchedEndpoints(); + + 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); +#if SEDP_VERBOSE + SEDP_LOG("Added new change to sedpPubWriter.\n"); +#endif + return (change != nullptr); +} + +template +bool SEDPAgent::disposeEndpointInSEDPHistory(A *local_endpoint, Writer *sedp_writer) { + return sedp_writer->removeFromHistory(local_endpoint->getSEDPSequenceNumber()); +} + +template +bool SEDPAgent::announceEndpointDeletion(A *local_endpoint, Writer *sedp_endpoint) { + 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); +#if SEDP_VERBOSE + SEDP_LOG("Added new change to sedpSubWriter.\n"); +#endif + return (change != nullptr); +} diff --git a/components/rtps_embedded/src/discovery/SPDPAgent.cpp b/components/rtps_embedded/src/discovery/SPDPAgent.cpp new file mode 100644 index 000000000..cb43428a0 --- /dev/null +++ b/components/rtps_embedded/src/discovery/SPDPAgent.cpp @@ -0,0 +1,350 @@ +/* +The MIT License +Copyright (c) 2019 Lehrstuhl Informatik 11 - RWTH Aachen University +Modifications Copyright (c) 2026 ATDev +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: +The above copyright notice and this permission notice shall be included in +all copies or substantial portions of the Software. +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +THE SOFTWARE + +This file is part of embeddedRTPS. + +Author: i11 - Embedded Software, RWTH Aachen University +*/ + +#include "rtps/discovery/SPDPAgent.hpp" +#include "rtps/discovery/ParticipantProxyData.hpp" +#include "rtps/entities/Participant.hpp" +#include "rtps/entities/Reader.hpp" +#include "rtps/entities/Writer.hpp" +#include "rtps/messages/MessageTypes.hpp" +#include "rtps/utils/Log.hpp" +#include "rtps/utils/udpUtils.hpp" + +#include +#include +#include + +using rtps::SPDPAgent; +using rtps::SMElement::BuildInEndpointSet; +using rtps::SMElement::ParameterId; + +SPDPAgent::SPDPAgent() + : espp::BaseComponent("RtpsSPDP", espp::Logger::Verbosity::WARN) {} + +void SPDPAgent::init(Participant &participant, BuiltInEndpoints &endpoints) { + mp_participant = &participant; + m_buildInEndpoints = endpoints; + m_buildInEndpoints.spdpReader->registerCallback(receiveCallback, this); + + 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..e683d0e3f --- /dev/null +++ b/components/rtps_embedded/src/discovery/TopicData.cpp @@ -0,0 +1,257 @@ +/* +The MIT License +Copyright (c) 2019 Lehrstuhl Informatik 11 - RWTH Aachen University +Modifications Copyright (c) 2026 ATDev +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: +The above copyright notice and this permission notice shall be included in +all copies or substantial portions of the Software. +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +THE SOFTWARE + +This file is part of embeddedRTPS. + +Author: i11 - Embedded Software, RWTH Aachen University +*/ +#include "rtps/discovery/TopicData.hpp" +#include "logger.hpp" +#include "rtps/messages/MessageTypes.hpp" +#include +#include + +using rtps::TopicData; +using rtps::TopicDataCompressed; +using rtps::SMElement::ParameterId; + +namespace { +espp::Logger s_topic_data_logger({.tag = "RtpsTopicData", .level = espp::Logger::Verbosity::WARN}); +} + +bool TopicData::isDisposedFlagSet() const { return statusInfoValid && ((statusInfo & 0b1)); } + +bool TopicData::isUnregisteredFlagSet() const { + return statusInfoValid && ((statusInfo & (0b1 << 1)) != 0); +} + +bool TopicData::matchesTopicOf(const TopicData &other) { + return strcmp(this->topicName, other.topicName) == 0 && + strcmp(this->typeName, other.typeName) == 0; +} + +bool TopicData::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); + if (topicNameLength > Config::MAX_TOPICNAME_LENGTH) { + s_topic_data_logger.warn("Topic name length {} exceeds maximum allowed length {}", + topicNameLength, Config::MAX_TOPICNAME_LENGTH); + return false; + } + ucdr_deserialize_array_char(&buffer, topicName, topicNameLength); + break; + case ParameterId::PID_TYPE_NAME: + uint32_t typeNameLength; + ucdr_deserialize_uint32_t(&buffer, &typeNameLength); + if (typeNameLength > Config::MAX_TYPENAME_LENGTH) { + s_topic_data_logger.warn("Type name length {} exceeds maximum allowed length {}", + typeNameLength, Config::MAX_TYPENAME_LENGTH); + return false; + } + 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..fd81bc19d --- /dev/null +++ b/components/rtps_embedded/src/entities/Domain.cpp @@ -0,0 +1,556 @@ +/* +The MIT License +Copyright (c) 2019 Lehrstuhl Informatik 11 - RWTH Aachen University +Modifications Copyright (c) 2026 ATDev +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: +The above copyright notice and this permission notice shall be included in +all copies or substantial portions of the Software. +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +THE SOFTWARE + +This file is part of embeddedRTPS. + +Author: i11 - Embedded Software, RWTH Aachen University +*/ + +#include "rtps/entities/Domain.hpp" +#include "rtps/utils/Log.hpp" +#include "rtps/utils/udpUtils.hpp" +#include +#include + +#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) { + m_transportSetupOk = 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) { + m_transportSetupOk = initializeTransport(); +} + +bool Domain::initializeTransport() { + assert(m_transport != nullptr); + bool success = true; + success = m_transport->ensureReceivePort(getUserMulticastPort()) && success; + success = m_transport->ensureReceivePort(getBuiltInMulticastPort()) && success; + success = m_transport->joinMultiCastGroup({239, 255, 0, 1}) && success; + return success; +} + +Domain::~Domain() { stop(); } + +bool Domain::completeInit() { + if (!m_transportSetupOk) { + DOMAIN_LOG("Failed transport setup. Domain initialization aborted."); + m_initComplete = false; + return false; + } + + m_initComplete = m_threadPool.startThreads(); + + if (!m_initComplete) { + DOMAIN_LOG("Failed starting threads"); + return false; + } + + for (auto i = 0; i < m_nextParticipantId; i++) { + m_participants[i].getSPDPAgent().start(); + } + return m_initComplete; +} + +void Domain::stop() { + for (auto i = PARTICIPANT_START_ID; i < m_nextParticipantId; ++i) { + m_participants[i - PARTICIPANT_START_ID].getSPDPAgent().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}, + LocatorIPv4(getBuiltInMulticastLocator()), + false}); + + TopicData spdpReaderAttributes; + spdpReaderAttributes.endpointGuid = {part.m_guidPrefix, ENTITYID_SPDP_BUILTIN_PARTICIPANT_READER}; + spdpReader->init(spdpReaderAttributes); + + // SEDP + + // Prepare attributes + TopicData sedpAttributes; + sedpAttributes.topicName[0] = '\0'; + sedpAttributes.typeName[0] = '\0'; + sedpAttributes.reliabilityKind = ReliabilityKind_t::RELIABLE; + sedpAttributes.durabilityKind = DurabilityKind_t::TRANSIENT_LOCAL; + sedpAttributes.endpointGuid.prefix = part.m_guidPrefix; + sedpAttributes.unicastLocator = getBuiltInUnicastLocator(part.m_participantId, m_localIpAddress); + + // READER + StatefulReader *sedpPubReader = + getNextUnusedEndpoint(m_statefulReaders); + sedpAttributes.endpointGuid.entityId = ENTITYID_SEDP_BUILTIN_PUBLICATIONS_READER; + sedpPubReader->init(sedpAttributes, *m_transport); + + StatefulReader *sedpSubReader = + getNextUnusedEndpoint(m_statefulReaders); + sedpAttributes.endpointGuid.entityId = ENTITYID_SEDP_BUILTIN_SUBSCRIPTIONS_READER; + sedpSubReader->init(sedpAttributes, *m_transport); + + // WRITER + StatefulWriter *sedpPubWriter = + getNextUnusedEndpoint(m_statefulWriters); + sedpAttributes.endpointGuid.entityId = ENTITYID_SEDP_BUILTIN_PUBLICATIONS_WRITER; + sedpPubWriter->init(sedpAttributes, TopicKind_t::NO_KEY, &m_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_transportSetupOk = m_transport->ensureReceivePort(getUserUnicastPort(part.m_participantId)) && + m_transportSetupOk; + m_transportSetupOk = + m_transport->ensureReceivePort(getBuiltInUnicastPort(part.m_participantId)) && + m_transportSetupOk; + m_threadPool.addBuiltinPort(getBuiltInUnicastPort(part.m_participantId)); +} + +void Domain::registerMulticastPort(FullLengthLocator mcastLocator) { + if (mcastLocator.kind == LocatorKind_t::LOCATOR_KIND_UDPv4) { + m_transportSetupOk = + m_transport->ensureReceivePort(mcastLocator.getLocatorPort()) && m_transportSetupOk; + } +} + +rtps::Reader *Domain::readerExists(Participant &part, const char *topicName, const char *typeName, + bool reliable) { + std::lock_guard lock(m_mutex); + if (reliable) { + for (unsigned int i = 0; i < m_statefulReaders.size(); i++) { + if (m_statefulReaders[i].isInitialized()) { + if (strncmp(m_statefulReaders[i].m_attributes.topicName, topicName, + Config::MAX_TYPENAME_LENGTH) != 0) { + continue; + } + + if (strncmp(m_statefulReaders[i].m_attributes.typeName, typeName, + Config::MAX_TYPENAME_LENGTH) != 0) { + continue; + } + + DOMAIN_LOG("StatefulReader exists already [{}, {}]", topicName, typeName); + + return &m_statefulReaders[i]; + } + } + } else { + for (unsigned int i = 0; i < m_statelessReaders.size(); i++) { + if (m_statelessReaders[i].isInitialized()) { + if (strncmp(m_statelessReaders[i].m_attributes.topicName, topicName, + Config::MAX_TYPENAME_LENGTH) != 0) { + continue; + } + + if (strncmp(m_statelessReaders[i].m_attributes.typeName, typeName, + Config::MAX_TYPENAME_LENGTH) != 0) { + continue; + } + + DOMAIN_LOG("StatelessReader exists [{}, {}]", topicName, typeName); + + return &m_statelessReaders[i]; + } + } + } + + return nullptr; +} + +rtps::Writer *Domain::writerExists(Participant &part, const char *topicName, const char *typeName, + bool reliable) { + std::lock_guard lock(m_mutex); + if (reliable) { + for (unsigned int i = 0; i < m_statefulWriters.size(); i++) { + if (m_statefulWriters[i].isInitialized()) { + if (strncmp(m_statefulWriters[i].m_attributes.topicName, topicName, + Config::MAX_TYPENAME_LENGTH) != 0) { + continue; + } + + if (strncmp(m_statefulWriters[i].m_attributes.typeName, typeName, + Config::MAX_TYPENAME_LENGTH) != 0) { + continue; + } + + DOMAIN_LOG("StatefulWriter exists [{}, {}]", topicName, typeName); + + return &m_statefulWriters[i]; + } + } + } else { + for (unsigned int i = 0; i < m_statelessWriters.size(); i++) { + if (m_statelessWriters[i].isInitialized()) { + if (strncmp(m_statelessWriters[i].m_attributes.topicName, topicName, + Config::MAX_TYPENAME_LENGTH) != 0) { + continue; + } + + if (strncmp(m_statelessWriters[i].m_attributes.typeName, typeName, + Config::MAX_TYPENAME_LENGTH) != 0) { + continue; + } + + DOMAIN_LOG("StatelessWriter exists [{}, {}]", topicName, typeName); + + return &m_statelessWriters[i]; + } + } + } + + return nullptr; +} + +rtps::Writer *Domain::createWriter(Participant &part, const char *topicName, const char *typeName, + bool reliable, bool enforceUnicast) { + std::lock_guard lock(m_mutex); + StatelessWriter *statelessWriter = + getNextUnusedEndpoint(m_statelessWriters); + StatefulWriter *statefulWriter = + getNextUnusedEndpoint(m_statefulWriters); + + // Check if there is enough capacity for more writers + if ((reliable && statefulWriter == nullptr) || (!reliable && statelessWriter == nullptr) || + part.isWritersFull()) { + + DOMAIN_LOG("No Writer created. Max Number of Writers reached."); + + return nullptr; + } + + // TODO Distinguish WithKey and NoKey (Also changes EntityKind) + TopicData attributes; + + if (strlen(topicName) >= Config::MAX_TOPICNAME_LENGTH || + strlen(typeName) >= Config::MAX_TYPENAME_LENGTH) { + return nullptr; + } + strncpy(attributes.topicName, topicName, Config::MAX_TOPICNAME_LENGTH); + strncpy(attributes.typeName, typeName, Config::MAX_TYPENAME_LENGTH); + attributes.topicName[Config::MAX_TOPICNAME_LENGTH - 1] = '\0'; + attributes.typeName[Config::MAX_TYPENAME_LENGTH - 1] = '\0'; + attributes.endpointGuid.prefix = part.m_guidPrefix; + attributes.endpointGuid.entityId = {part.getNextUserEntityKey(), + EntityKind_t::USER_DEFINED_WRITER_WITHOUT_KEY}; + attributes.unicastLocator = getUserUnicastLocator(part.m_participantId, m_localIpAddress); + attributes.durabilityKind = DurabilityKind_t::TRANSIENT_LOCAL; + + DOMAIN_LOG("Creating writer[{}, {}]", topicName, typeName); + + if (reliable) { + attributes.reliabilityKind = ReliabilityKind_t::RELIABLE; + + if (!statefulWriter->init(attributes, TopicKind_t::NO_KEY, &m_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; + } + strncpy(attributes.topicName, topicName, Config::MAX_TOPICNAME_LENGTH); + strncpy(attributes.typeName, typeName, Config::MAX_TYPENAME_LENGTH); + attributes.topicName[Config::MAX_TOPICNAME_LENGTH - 1] = '\0'; + attributes.typeName[Config::MAX_TYPENAME_LENGTH - 1] = '\0'; + attributes.endpointGuid.prefix = part.m_guidPrefix; + attributes.endpointGuid.entityId = {part.getNextUserEntityKey(), + EntityKind_t::USER_DEFINED_READER_WITHOUT_KEY}; + attributes.unicastLocator = getUserUnicastLocator(part.m_participantId, m_localIpAddress); + if (!isZeroAddress(mcastaddress)) { + if (isMulticastAddress(mcastaddress)) { + attributes.multicastLocator = rtps::FullLengthLocator::createUDPv4Locator( + mcastaddress[0], mcastaddress[1], mcastaddress[2], mcastaddress[3], + getUserMulticastPort()); + m_transportSetupOk = + m_transport->joinMultiCastGroup( + {attributes.multicastLocator.address[12], attributes.multicastLocator.address[13], + attributes.multicastLocator.address[14], attributes.multicastLocator.address[15]}) && + m_transportSetupOk; + registerMulticastPort(attributes.multicastLocator); + + DOMAIN_LOG("Multicast enabled!"); + + } else { + + DOMAIN_LOG("This is not a Multicastaddress!"); + } + } + attributes.durabilityKind = DurabilityKind_t::VOLATILE; + + DOMAIN_LOG("Creating reader[{}, {}]", topicName, typeName); + + if (reliable) { + + attributes.reliabilityKind = ReliabilityKind_t::RELIABLE; + + statefulReader->init(attributes, *m_transport); + + if (!part.addReader(statefulReader)) { + DOMAIN_LOG("Failed to add reader to participant."); + + return nullptr; + } + return statefulReader; + } else { + + attributes.reliabilityKind = ReliabilityKind_t::BEST_EFFORT; + + statelessReader->init(attributes); + + if (!part.addReader(statelessReader)) { + return nullptr; + } + return statelessReader; + } +} + +bool rtps::Domain::deleteReader(Participant &part, Reader *reader) { + std::lock_guard lock(m_mutex); + if (reader == nullptr || !reader->isInitialized()) { + return false; + } + if (!part.deleteReader(reader)) { + return false; + } + + reader->reset(); + return true; +} + +bool rtps::Domain::deleteWriter(Participant &part, Writer *writer) { + std::lock_guard lock(m_mutex); + if (writer == nullptr || !writer->isInitialized()) { + return false; + } + if (!part.deleteWriter(writer)) { + return false; + } + + writer->reset(); + return true; +} + +void rtps::Domain::printInfo() { + for (unsigned int i = 0; i < m_participants.size(); i++) { + DOMAIN_LOG("Participant {}", i); + m_participants[i].printInfo(); + } +} + +rtps::GuidPrefix_t Domain::generateGuidPrefix(ParticipantId_t id) const { + GuidPrefix_t prefix; +#if defined(ESP_PLATFORM) + uint8_t mac[6] = {0}; + esp_err_t mac_err = esp_read_mac(mac, ESP_MAC_ETH); + if (mac_err != ESP_OK) { + mac_err = esp_read_mac(mac, ESP_MAC_WIFI_STA); + } + if (mac_err == ESP_OK) { + // Make participant GUID unique per board while keeping a stable layout. + prefix.id[0] = mac[0]; + prefix.id[1] = mac[1]; + prefix.id[2] = mac[2]; + prefix.id[3] = mac[3]; + prefix.id[4] = mac[4]; + prefix.id[5] = mac[5]; + prefix.id[6] = static_cast(id); + prefix.id[7] = Config::VENDOR_ID.vendorId[0]; + prefix.id[8] = Config::VENDOR_ID.vendorId[1]; + prefix.id[9] = Config::DOMAIN_ID; + prefix.id[10] = 0xA5; + prefix.id[11] = 0x5A; + return prefix; + } +#endif + + if (Config::BASE_GUID_PREFIX == GUID_RANDOM) { + 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..2317df823 --- /dev/null +++ b/components/rtps_embedded/src/entities/Participant.cpp @@ -0,0 +1,518 @@ +/* +The MIT License +Copyright (c) 2019 Lehrstuhl Informatik 11 - RWTH Aachen University +Modifications Copyright (c) 2026 ATDev +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: +The above copyright notice and this permission notice shall be included in +all copies or substantial portions of the Software. +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +THE SOFTWARE + +This file is part of embeddedRTPS. + +Author: i11 - Embedded Software, RWTH Aachen University +*/ + +#include "rtps/entities/Participant.hpp" +#include "rtps/entities/Reader.hpp" +#include "rtps/entities/Writer.hpp" +#include "rtps/messages/MessageReceiver.hpp" +#include "rtps/utils/Log.hpp" +#include + +#if PARTICIPANT_VERBOSE && RTPS_GLOBAL_VERBOSE +#define PARTICIPANT_LOG(...) logger_.warn(__VA_ARGS__) +#else +#define PARTICIPANT_LOG(...) \ + do { \ + } while (0) +#endif + +using rtps::Participant; + +Participant::Participant() + : espp::BaseComponent("RtpsParticipant", espp::Logger::Verbosity::WARN) + , m_guidPrefix(GUIDPREFIX_UNKNOWN) + , m_participantId(PARTICIPANT_ID_INVALID) + , m_receiver(this) {} +Participant::Participant(const GuidPrefix_t &guidPrefix, ParticipantId_t participantId) + : espp::BaseComponent("RtpsParticipant", espp::Logger::Verbosity::WARN) + , m_guidPrefix(guidPrefix) + , m_participantId(participantId) + , m_receiver(this) {} + +Participant::~Participant() { m_spdpAgent.stop(); } + +void Participant::reuse(const GuidPrefix_t &guidPrefix, ParticipantId_t participantId) { + m_guidPrefix = guidPrefix; + m_participantId = participantId; + m_localIpAddress = {Config::IP_ADDRESS[0], Config::IP_ADDRESS[1], Config::IP_ADDRESS[2], + Config::IP_ADDRESS[3]}; +} + +void Participant::reuse(const GuidPrefix_t &guidPrefix, ParticipantId_t participantId, + const Ip4AddressBytes &localIpAddress) { + m_guidPrefix = guidPrefix; + m_participantId = participantId; + m_localIpAddress = localIpAddress; +} + +bool Participant::isValid() { return m_participantId != PARTICIPANT_ID_INVALID; } + +std::array Participant::getNextUserEntityKey() { + const auto result = m_nextUserEntityId; + + ++m_nextUserEntityId[2]; + if (m_nextUserEntityId[2] == 0) { + ++m_nextUserEntityId[1]; + if (m_nextUserEntityId[1] == 0) { + ++m_nextUserEntityId[0]; + } + } + return result; +} + +bool Participant::registerOnNewPublisherMatchedCallback(void (*callback)(void *arg), void *args) { + if (!m_hasBuilInEndpoints) { + return false; + } + + m_sedpAgent.registerOnNewPublisherMatchedCallback(callback, args); + return true; +} + +bool Participant::registerOnNewSubscriberMatchedCallback(void (*callback)(void *arg), void *args) { + if (!m_hasBuilInEndpoints) { + return false; + } + + m_sedpAgent.registerOnNewSubscriberMatchedCallback(callback, args); + return true; +} + +rtps::Writer *Participant::addWriter(Writer *pWriter) { + std::lock_guard lock(m_mutex); + for (unsigned int i = 0; i < m_writers.size(); i++) { + if (m_writers[i] == nullptr) { + m_writers[i] = pWriter; + if (m_hasBuilInEndpoints) { + m_sedpAgent.addWriter(*pWriter); + } + return pWriter; + } + } + return nullptr; +} + +bool Participant::isWritersFull() { + std::lock_guard lock(m_mutex); + for (unsigned int i = 0; i < m_writers.size(); i++) { + if (m_writers[i] == nullptr) { + return false; + } + } + + return true; +} + +rtps::Reader *Participant::addReader(Reader *pReader) { + std::lock_guard lock(m_mutex); + for (unsigned int i = 0; i < m_readers.size(); i++) { + if (m_readers[i] == nullptr) { + m_readers[i] = pReader; + if (m_hasBuilInEndpoints) { + m_sedpAgent.addReader(*pReader); + } + return pReader; + } + } + + return nullptr; +} + +bool Participant::deleteReader(Reader *reader) { + std::lock_guard lock(m_mutex); + for (unsigned int i = 0; i < m_readers.size(); i++) { + if (m_readers[i]->getSEDPSequenceNumber() == reader->getSEDPSequenceNumber()) { + if (m_sedpAgent.deleteReader(reader)) { + m_readers[i] = nullptr; + return true; + } + PARTICIPANT_LOG("Found reader but SEDP deletion failed"); + } + } + return false; +} + +bool Participant::deleteWriter(Writer *writer) { + std::lock_guard lock(m_mutex); + for (unsigned int i = 0; i < m_writers.size(); i++) { + if (m_writers[i]->getSEDPSequenceNumber() == writer->getSEDPSequenceNumber()) { + if (m_sedpAgent.deleteWriter(writer)) { + m_writers[i] = nullptr; + return true; + } + PARTICIPANT_LOG("Found reader but SEDP deletion failed"); + } + } + return false; +} + +bool Participant::isReadersFull() { + std::lock_guard lock(m_mutex); + for (unsigned int i = 0; i < m_readers.size(); i++) { + if (m_readers[i] == nullptr) { + return false; + } + } + + return true; +} + +rtps::Writer *Participant::getWriter(EntityId_t id) { + std::lock_guard lock(m_mutex); + for (size_t i = 0; i < m_writers.size(); ++i) { + if (m_writers[i] == nullptr) { + continue; + } + if (m_writers[i]->m_attributes.endpointGuid.entityId == id) { + return m_writers[i]; + } + } + return nullptr; +} + +rtps::Reader *Participant::getReader(EntityId_t id) { + std::lock_guard lock(m_mutex); + for (size_t i = 0; i < m_readers.size(); ++i) { + if (m_readers[i] == nullptr) { + continue; + } + if (m_readers[i]->m_attributes.endpointGuid.entityId == id) { + return m_readers[i]; + } + } + return nullptr; +} + +rtps::Reader *Participant::getReaderByWriterId(const Guid_t &guid) { + std::lock_guard lock(m_mutex); + for (size_t i = 0; i < m_readers.size(); ++i) { + if (m_readers[i] == nullptr) { + continue; + } + if (m_readers[i]->isProxy(guid)) { + return m_readers[i]; + } + } + return nullptr; +} + +rtps::Writer *Participant::getMatchingWriter(const TopicData &readerTopicData) { + std::lock_guard lock(m_mutex); + for (size_t i = 0; i < m_writers.size(); ++i) { + if (m_writers[i] == nullptr) { + continue; + } + if (m_writers[i]->m_attributes.matchesTopicOf(readerTopicData) && + (readerTopicData.reliabilityKind == ReliabilityKind_t::BEST_EFFORT || + m_writers[i]->m_attributes.reliabilityKind == ReliabilityKind_t::RELIABLE)) { + return m_writers[i]; + } + } + return nullptr; +} + +rtps::Reader *Participant::getMatchingReader(const TopicData &writerTopicData) { + std::lock_guard lock(m_mutex); + for (size_t i = 0; i < m_readers.size(); ++i) { + if (m_readers[i] == nullptr) { + continue; + } + if (m_readers[i]->m_attributes.matchesTopicOf(writerTopicData) && + (writerTopicData.reliabilityKind == ReliabilityKind_t::RELIABLE || + m_readers[i]->m_attributes.reliabilityKind == ReliabilityKind_t::BEST_EFFORT)) { + return m_readers[i]; + } + } + return nullptr; +} + +rtps::Writer *Participant::getMatchingWriter(const TopicDataCompressed &readerTopicData) { + std::lock_guard lock(m_mutex); + for (size_t i = 0; i < m_writers.size(); ++i) { + if (m_writers[i] == nullptr) { + continue; + } + if (readerTopicData.matchesTopicOf(m_writers[i]->m_attributes) && + (readerTopicData.is_reliable == false || + m_writers[i]->m_attributes.reliabilityKind == ReliabilityKind_t::RELIABLE)) { + return m_writers[i]; + } + } + return nullptr; +} + +rtps::Reader *Participant::getMatchingReader(const TopicDataCompressed &writerTopicData) { + std::lock_guard lock(m_mutex); + for (size_t i = 0; i < m_readers.size(); ++i) { + if (m_readers[i] == nullptr) { + continue; + } + if (writerTopicData.matchesTopicOf(m_readers[i]->m_attributes) && + (writerTopicData.is_reliable == true || + m_readers[i]->m_attributes.reliabilityKind == ReliabilityKind_t::BEST_EFFORT)) { + return m_readers[i]; + } + } + return nullptr; +} + +bool Participant::addNewRemoteParticipant(const ParticipantProxyData &remotePart) { + std::lock_guard lock(m_mutex); + return m_remoteParticipants.add(remotePart); +} + +bool Participant::removeRemoteParticipant(const GuidPrefix_t &prefix) { + std::lock_guard lock(m_mutex); + auto isElementToRemove = [&](const ParticipantProxyData &proxy) { + return proxy.m_guid.prefix == prefix; + }; + auto thunk = [](void *arg, const ParticipantProxyData &value) { + return (*static_cast(arg))(value); + }; + removeAllProxiesOfParticipant(prefix); + m_sedpAgent.removeUnmatchedEntitiesOfParticipant(prefix); + return m_remoteParticipants.remove(thunk, &isElementToRemove); +} + +void Participant::removeAllProxiesOfParticipant(const GuidPrefix_t &prefix) { + std::lock_guard lock(m_mutex); + for (unsigned int i = 0; i < m_readers.size(); i++) { + if (m_readers[i] == nullptr) { + continue; + } + m_readers[i]->removeAllProxiesOfParticipant(prefix); + } + + for (unsigned int i = 0; i < m_writers.size(); i++) { + if (m_writers[i] == nullptr) { + continue; + } + m_writers[i]->removeAllProxiesOfParticipant(prefix); + } +} + +void Participant::removeProxyFromAllEndpoints(const Guid_t &guid) { + std::lock_guard lock(m_mutex); + for (unsigned int i = 0; i < m_writers.size(); i++) { + if (m_writers[i] == nullptr) { + continue; + } + if (m_writers[i]->removeProxy(guid)) { + PARTICIPANT_LOG("Removing proxy for writer [{}, {}], proxies left = {}", + m_writers[i]->m_attributes.topicName, m_writers[i]->m_attributes.typeName, + (int)m_writers[i]->getProxiesCount()); + } + } + + for (unsigned int i = 0; i < m_readers.size(); i++) { + if (m_readers[i] == nullptr) { + continue; + } + if (m_readers[i]->removeProxy(guid)) { + PARTICIPANT_LOG("Removing proxy for reader [{}, {}], proxies left = {}", + m_readers[i]->m_attributes.topicName, m_readers[i]->m_attributes.typeName, + (int)m_readers[i]->getProxiesCount()); + } + } +} + +const rtps::ParticipantProxyData *Participant::findRemoteParticipant(const GuidPrefix_t &prefix) { + std::lock_guard lock(m_mutex); + auto isElementToFind = [&](const ParticipantProxyData &proxy) { + return proxy.m_guid.prefix == prefix; + }; + auto thunk = [](void *arg, const ParticipantProxyData &value) { + return (*static_cast(arg))(value); + }; + return m_remoteParticipants.find(thunk, &isElementToFind); +} + +void Participant::refreshRemoteParticipantLiveliness(const GuidPrefix_t &prefix) { + std::lock_guard lock(m_mutex); + auto isElementToFind = [&](const ParticipantProxyData &proxy) { + return proxy.m_guid.prefix == prefix; + }; + auto thunk = [](void *arg, const ParticipantProxyData &value) { + return (*static_cast(arg))(value); + }; + + auto remoteParticipant = m_remoteParticipants.find(thunk, &isElementToFind); + if (remoteParticipant != nullptr) { + remoteParticipant->onAliveSignal(); + } +} + +bool Participant::hasReaderWithMulticastLocator(const std::array &address) { + std::lock_guard lock(m_mutex); + for (size_t i = 0; i < m_readers.size(); i++) { + if (m_readers[i] == nullptr) { + continue; + } + if (m_readers[i]->m_attributes.multicastLocator.getIp4AddressBytes() == address) { + return true; + } + } + return false; +} + +uint32_t Participant::getRemoteParticipantCount() { + std::lock_guard lock(m_mutex); + return m_remoteParticipants.getNumElements(); +} + +rtps::MessageReceiver *Participant::getMessageReceiver() { return &m_receiver; } + +bool Participant::checkAndResetHeartbeats() { + std::lock_guard lock1(m_mutex); + std::lock_guard lock2(m_spdpAgent.m_mutex); + PARTICIPANT_LOG("Have {} remote participants", + (unsigned int)m_remoteParticipants.getNumElements()); + PARTICIPANT_LOG("Unmatched remote writers/readers, {} / {}", + static_cast(m_sedpAgent.getNumRemoteUnmatchedWriters()), + static_cast(m_sedpAgent.getNumRemoteUnmatchedReaders())); + for (auto &remote : m_remoteParticipants) { + PARTICIPANT_LOG("Remote GUID = {} {} {} {} | Age = {} [ms]", remote.m_guid.prefix.id[4], + remote.m_guid.prefix.id[5], remote.m_guid.prefix.id[6], + remote.m_guid.prefix.id[7], + (unsigned int)remote.getAliveSignalAgeInMilliseconds()); + if (remote.isAlive()) { + continue; + } + PARTICIPANT_LOG("removing remote participant"); + bool success = removeRemoteParticipant(remote.m_guid.prefix); + if (!success) { + return false; + } else { + return true; + } + } + return true; +} + +void Participant::printInfo() { + + uint32_t max_reader_proxies = 0; + for (unsigned int i = 0; i < m_readers.size(); i++) { + if (m_readers[i] != nullptr && m_readers[i]->isInitialized()) { + if (m_hasBuilInEndpoints && i < 3) { +#ifdef PARTICIPANT_PRINTINFO_LONG + if (m_readers[i]->m_attributes.endpointGuid.entityId == + ENTITYID_SPDP_BUILTIN_PARTICIPANT_READER) { + PARTICIPANT_LOG("Reader {}: SPDP BUILTIN READER | Remote Proxies = {}", i, + static_cast(m_readers[i]->getProxiesCount())); + } + if (m_readers[i]->m_attributes.endpointGuid.entityId == + ENTITYID_SEDP_BUILTIN_PUBLICATIONS_READER) { + PARTICIPANT_LOG("Reader {}: SEDP PUBLICATION READER | Remote Proxies = {}", i, + static_cast(m_readers[i]->getProxiesCount())); + } + if (m_readers[i]->m_attributes.endpointGuid.entityId == + ENTITYID_SEDP_BUILTIN_SUBSCRIPTIONS_READER) { + PARTICIPANT_LOG("Reader {}: SEDP SUBSCRIPTION READER | Remote Proxies = {}", i, + static_cast(m_readers[i]->getProxiesCount())); + } +#endif + continue; + } + + max_reader_proxies = std::max(max_reader_proxies, m_readers[i]->getProxiesCount()); +#ifdef PARTICIPANT_PRINTINFO_LONG + PARTICIPANT_LOG("Reader {}: Topic = {} | Type = {} | Remote Proxies = {} | SEDP " + "SN = {}", + i, m_readers[i]->m_attributes.topicName, m_readers[i]->m_attributes.typeName, + static_cast(m_readers[i]->getProxiesCount()), + static_cast(m_readers[i]->getSEDPSequenceNumber().low)); +#endif + } + } + + uint32_t max_writer_proxies = 0; + for (unsigned int i = 0; i < m_writers.size(); i++) { + + if (m_hasBuilInEndpoints && i < 3) { +#ifdef PARTICIPANT_PRINTINFO_LONG + if (m_writers[i]->m_attributes.endpointGuid.entityId == + ENTITYID_SPDP_BUILTIN_PARTICIPANT_WRITER) { + PARTICIPANT_LOG("Writer {}: SPDP WRITER | Remote Proxies = {}", i, + static_cast(m_writers[i]->getProxiesCount())); + } + if (m_writers[i]->m_attributes.endpointGuid.entityId == + ENTITYID_SEDP_BUILTIN_PUBLICATIONS_WRITER) { + PARTICIPANT_LOG("Writer {}: SEDP PUBLICATION WRITER | Remote Proxies = {}", i, + static_cast(m_writers[i]->getProxiesCount())); + } + if (m_writers[i]->m_attributes.endpointGuid.entityId == + ENTITYID_SEDP_BUILTIN_SUBSCRIPTIONS_WRITER) { + PARTICIPANT_LOG("Writer {}: SEDP SUBSCRIPTION WRITER | Remote Proxies = {}", i, + static_cast(m_writers[i]->getProxiesCount())); + } +#endif + continue; + } + + if (m_writers[i] != nullptr && m_writers[i]->isInitialized()) { + max_writer_proxies = std::max(max_writer_proxies, m_writers[i]->getProxiesCount()); +#ifdef PARTICIPANT_PRINTINFO_LONG + PARTICIPANT_LOG("Writer {}: Topic = {} | Type = {} | Remote Proxies = {} | SEDP " + "SN = {}", + i, m_writers[i]->m_attributes.topicName, m_writers[i]->m_attributes.typeName, + static_cast(m_writers[i]->getProxiesCount()), + static_cast(m_writers[i]->getSEDPSequenceNumber().low)); +#endif + } + } + + PARTICIPANT_LOG("Max Writer Proxies {}", max_writer_proxies); + PARTICIPANT_LOG("Max Reader Proxies {}", max_reader_proxies); + PARTICIPANT_LOG("Unmatched Remote Readers = {}", + static_cast(m_sedpAgent.getNumRemoteUnmatchedReaders())); + PARTICIPANT_LOG("Unmatched Remote Writers = {}", + static_cast(m_sedpAgent.getNumRemoteUnmatchedWriters())); + PARTICIPANT_LOG("Remote Participants = {}", + static_cast(m_remoteParticipants.getNumElements())); +} + +rtps::SPDPAgent &Participant::getSPDPAgent() { return m_spdpAgent; } + +void Participant::addBuiltInEndpoints(BuiltInEndpoints &endpoints) { + std::lock_guard lock(m_mutex); + m_hasBuilInEndpoints = true; + m_spdpAgent.init(*this, endpoints); + m_sedpAgent.init(*this, endpoints); + + // This needs to be done after initializing the agents + addWriter(endpoints.spdpWriter); + addReader(endpoints.spdpReader); + addWriter(endpoints.sedpPubWriter); + addReader(endpoints.sedpPubReader); + addWriter(endpoints.sedpSubWriter); + addReader(endpoints.sedpSubReader); +} + +void Participant::newMessage(const uint8_t *data, DataSize_t size) { + if (!m_receiver.processMessage(data, size)) { + PARTICIPANT_LOG("MESSAGE PROCESSING FAILED"); + } +} diff --git a/components/rtps_embedded/src/entities/Reader.cpp b/components/rtps_embedded/src/entities/Reader.cpp new file mode 100644 index 000000000..f2406955c --- /dev/null +++ b/components/rtps_embedded/src/entities/Reader.cpp @@ -0,0 +1,144 @@ +#include +#include +#include +#include +#include +#include + +using namespace rtps; + +Reader::Reader() + : espp::BaseComponent("RtpsReader", espp::Logger::Verbosity::WARN) { + m_callbacks.fill({nullptr, nullptr, 0}); +} + +void Reader::executeCallbacks(const ReaderCacheChange &cacheChange) { + std::lock_guard lock(m_callback_mutex); + for (unsigned int i = 0; i < m_callbacks.size(); i++) { + if (m_callbacks[i].function != nullptr) { + m_callbacks[i].function(m_callbacks[i].arg, cacheChange); + } + } +} + +bool Reader::initMutex() { return true; } + +void Reader::reset() { + std::lock_guard lock1(m_proxies_mutex); + std::lock_guard lock2(m_callback_mutex); + + m_proxies.clear(); + for (unsigned int i = 0; i < m_callbacks.size(); i++) { + m_callbacks[i].function = nullptr; + m_callbacks[i].arg = nullptr; + } + + m_callback_count = 0; + m_is_initialized_ = false; +} + +bool Reader::isProxy(const Guid_t &guid) { + for (const auto &proxy : m_proxies) { + if (proxy.remoteWriterGuid.operator==(guid)) { + return true; + } + } + return false; +} + +WriterProxy *Reader::getProxy(Guid_t guid) { + auto isElementToFind = [&](const WriterProxy &proxy) { return proxy.remoteWriterGuid == guid; }; + auto thunk = [](void *arg, const WriterProxy &value) { + return (*static_cast(arg))(value); + }; + return m_proxies.find(thunk, &isElementToFind); +} + +Reader::callbackIdentifier_t Reader::registerCallback(Reader::callbackFunction_t cb, void *arg) { + std::lock_guard lock(m_callback_mutex); + if (m_callback_count == m_callbacks.size() || cb == nullptr) { + return false; + } + + for (unsigned int i = 0; i < m_callbacks.size(); i++) { + if (m_callbacks[i].function == nullptr) { + m_callbacks[i].function = cb; + m_callbacks[i].arg = arg; + m_callbacks[i].identifier = m_callback_identifier++; + m_callback_count++; + return m_callbacks[i].identifier; + } + } + + return 0; +} + +uint32_t Reader::getProxiesCount() { return m_proxies.getNumElements(); } + +bool Reader::removeCallback(Reader::callbackIdentifier_t identifier) { + std::lock_guard lock(m_callback_mutex); + for (unsigned int i = 0; i < m_callbacks.size(); i++) { + if (m_callbacks[i].identifier == identifier) { + m_callbacks[i].function = nullptr; + m_callbacks[i].arg = nullptr; + m_callback_count--; + return true; + } + } + + return false; +} + +uint8_t Reader::getNumCallbacks() { return m_callback_count; } + +void Reader::removeAllProxiesOfParticipant(const GuidPrefix_t &guidPrefix) { + std::lock_guard lock(m_proxies_mutex); + auto isElementToRemove = [&](const WriterProxy &proxy) { + return proxy.remoteWriterGuid.prefix == guidPrefix; + }; + auto thunk = [](void *arg, const WriterProxy &value) { + return (*static_cast(arg))(value); + }; + + m_proxies.remove(thunk, &isElementToRemove); +} + +bool Reader::removeProxy(const Guid_t &guid) { + std::lock_guard lock(m_proxies_mutex); + auto isElementToRemove = [&](const WriterProxy &proxy) { return proxy.remoteWriterGuid == guid; }; + auto thunk = [](void *arg, const WriterProxy &value) { + return (*static_cast(arg))(value); + }; + + return m_proxies.remove(thunk, &isElementToRemove); +} + +bool Reader::addNewMatchedWriter(const WriterProxy &newProxy) { + std::lock_guard lock(m_proxies_mutex); +#if (SFR_VERBOSE || SLR_VERBOSE) && RTPS_GLOBAL_VERBOSE + SFR_LOG("New writer added with id: "); + printGuid(newProxy.remoteWriterGuid); +#endif + return m_proxies.add(newProxy); +} + +void rtps::Reader::setSEDPSequenceNumber(const SequenceNumber_t &sn) { + m_sedp_sequence_number = sn; +} +const rtps::SequenceNumber_t &rtps::Reader::getSEDPSequenceNumber() { + return m_sedp_sequence_number; +} + +int rtps::Reader::dumpAllProxies(dumpProxyCallback target, void *arg) { + if (target == nullptr) { + return 0; + } + std::lock_guard lock(m_proxies_mutex); + int dump_count = 0; + for (auto it = m_proxies.begin(); it != m_proxies.end(); ++it, ++dump_count) { + target(this, *it, arg); + } + return dump_count; +} + +bool rtps::Reader::sendPreemptiveAckNack(const WriterProxy &writer) { return true; } diff --git a/components/rtps_embedded/src/entities/StatelessReader.cpp b/components/rtps_embedded/src/entities/StatelessReader.cpp new file mode 100644 index 000000000..c63799a9d --- /dev/null +++ b/components/rtps_embedded/src/entities/StatelessReader.cpp @@ -0,0 +1,78 @@ +/* +The MIT License +Copyright (c) 2019 Lehrstuhl Informatik 11 - RWTH Aachen University +Modifications Copyright (c) 2026 ATDev +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: +The above copyright notice and this permission notice shall be included in +all copies or substantial portions of the Software. +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +THE SOFTWARE + +This file is part of embeddedRTPS. + +Author: i11 - Embedded Software, RWTH Aachen University +*/ + +#include "rtps/entities/StatelessReader.hpp" +#include "rtps/utils/Log.hpp" + +using rtps::StatelessReader; + +#if SLR_VERBOSE && RTPS_GLOBAL_VERBOSE +#include "rtps/utils/printutils.hpp" +#define SLR_LOG(...) \ + if (true) { \ + printf("[StatelessReader %s] ", &m_attributes.topicName[0]); \ + printf(__VA_ARGS__); \ + printf("\n"); \ + } +#else +#define SLR_LOG(...) // +#endif + +bool StatelessReader::init(const TopicData &attributes) { + if (!initMutex()) { + return false; + } + + m_proxies.clear(); + m_attributes = attributes; + m_is_initialized_ = true; + return true; +} + +void StatelessReader::newChange(const ReaderCacheChange &cacheChange) { + if (!m_is_initialized_) { + return; + } + executeCallbacks(cacheChange); +} + +bool StatelessReader::addNewMatchedWriter(const WriterProxy &newProxy) { +#if (SLR_VERBOSE && RTPS_GLOBAL_VERBOSE) + SLR_LOG("Adding WriterProxy"); + printGuid(newProxy.remoteWriterGuid); +#endif + return m_proxies.add(newProxy); +} + +bool StatelessReader::onNewHeartbeat(const SubmessageHeartbeat &, const GuidPrefix_t &) { + // nothing to do + return true; +} + +bool StatelessReader::onNewGapMessage(const SubmessageGap &msg, const GuidPrefix_t &remotePrefix) { + return true; +} + +#undef SLR_VERBOSE diff --git a/components/rtps_embedded/src/entities/Writer.cpp b/components/rtps_embedded/src/entities/Writer.cpp new file mode 100644 index 000000000..6d1e2ff37 --- /dev/null +++ b/components/rtps_embedded/src/entities/Writer.cpp @@ -0,0 +1,144 @@ +#include "rtps/utils/Log.hpp" +#include +#include +#include +#include +#include +#include + +using namespace rtps; + +Writer::Writer() + : espp::BaseComponent("RtpsWriter", espp::Logger::Verbosity::WARN) {} + +bool rtps::Writer::addNewMatchedReader(const ReaderProxy &newProxy) { + INIT_GUARD(); +#if SFW_VERBOSE && RTPS_GLOBAL_VERBOSE + SFW_LOG("New reader added with id: "); + printGuid(newProxy.remoteReaderGuid); +#endif + std::lock_guard lock(m_mutex); + bool success = m_proxies.add(newProxy); + if (!m_enforceUnicast) { + manageSendOptions(); + } + return success; +} + +bool rtps::Writer::removeProxy(const Guid_t &guid) { + INIT_GUARD() + std::lock_guard lock(m_mutex); + auto isElementToRemove = [&](const ReaderProxy &proxy) { return proxy.remoteReaderGuid == guid; }; + auto thunk = [](void *arg, const ReaderProxy &value) { + return (*static_cast(arg))(value); + }; + + bool ret = m_proxies.remove(thunk, &isElementToRemove); + resetSendOptions(); + return ret; +} + +uint32_t rtps::Writer::getProxiesCount() { + std::lock_guard lock(m_mutex); + return m_proxies.getNumElements(); +} + +void rtps::Writer::resetSendOptions() { + INIT_GUARD() + for (auto &proxy : m_proxies) { + proxy.suppressUnicast = false; + proxy.useMulticast = false; + proxy.unknown_eid = false; + } + manageSendOptions(); +} + +const rtps::CacheChange *rtps::Writer::newChange(ChangeKind_t kind, const uint8_t *data, + DataSize_t size) { + return newChange(kind, data, size, false, false); +} + +void rtps::Writer::manageSendOptions() { + INIT_GUARD(); + std::lock_guard lock(m_mutex); + for (auto &proxy : m_proxies) { + if (proxy.remoteMulticastLocator.kind == LocatorKind_t::LOCATOR_KIND_INVALID) { + proxy.suppressUnicast = false; + proxy.useMulticast = false; + } else { + bool found = false; + for (auto &avproxy : m_proxies) { + if (avproxy.remoteMulticastLocator.kind == LocatorKind_t::LOCATOR_KIND_UDPv4 && + avproxy.remoteMulticastLocator.getIp4AddressBytes() == + proxy.remoteMulticastLocator.getIp4AddressBytes() && + avproxy.remoteLocator.getIp4AddressBytes() != + proxy.remoteLocator.getIp4AddressBytes()) { + if (avproxy.suppressUnicast == false) { + avproxy.useMulticast = false; + avproxy.suppressUnicast = true; + proxy.useMulticast = true; + proxy.suppressUnicast = true; + if (avproxy.remoteReaderGuid.entityId != proxy.remoteReaderGuid.entityId) { + proxy.unknown_eid = true; + } + } + found = true; + } + } + if (!found) { + proxy.useMulticast = false; + proxy.suppressUnicast = false; + } + } + } +} + +void rtps::Writer::removeAllProxiesOfParticipant(const GuidPrefix_t &guidPrefix) { + INIT_GUARD(); + std::lock_guard lock(m_mutex); + auto isElementToRemove = [&](const ReaderProxy &proxy) { + return proxy.remoteReaderGuid.prefix == guidPrefix; + }; + auto thunk = [](void *arg, const ReaderProxy &value) { + return (*static_cast(arg))(value); + }; + + m_proxies.remove(thunk, &isElementToRemove); + resetSendOptions(); +} + +bool rtps::Writer::isBuiltinEndpoint() { + return !(m_attributes.endpointGuid.entityId.entityKind == + EntityKind_t::USER_DEFINED_WRITER_WITHOUT_KEY || + m_attributes.endpointGuid.entityId.entityKind == + EntityKind_t::USER_DEFINED_WRITER_WITH_KEY); +} + +bool rtps::Writer::isIrrelevant(ChangeKind_t kind) const { + // Right now we only allow alive changes + // return kind == ChangeKind_t::INVALID || (m_topicKind == TopicKind_t::NO_KEY + // && kind != ChangeKind_t::ALIVE); + return kind != ChangeKind_t::ALIVE; +} + +bool rtps::Writer::isInitialized() { return m_is_initialized_; } + +void rtps::Writer::setSEDPSequenceNumber(const SequenceNumber_t &sn) { + m_sedp_sequence_number = sn; +} + +const rtps::SequenceNumber_t &rtps::Writer::getSEDPSequenceNumber() { + return m_sedp_sequence_number; +} + +int rtps::Writer::dumpAllProxies(dumpProxyCallback target, void *arg) { + if (target == nullptr) { + return 0; + } + std::lock_guard lock(m_mutex); + int dump_count = 0; + for (auto it = m_proxies.begin(); it != m_proxies.end(); ++it, ++dump_count) { + target(this, *it, arg); + } + return dump_count; +} diff --git a/components/rtps_embedded/src/messages/MessageReceiver.cpp b/components/rtps_embedded/src/messages/MessageReceiver.cpp new file mode 100644 index 000000000..46d11ad2d --- /dev/null +++ b/components/rtps_embedded/src/messages/MessageReceiver.cpp @@ -0,0 +1,279 @@ +/* +The MIT License +Copyright (c) 2019 Lehrstuhl Informatik 11 - RWTH Aachen University +Modifications Copyright (c) 2026 ATDev +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: +The above copyright notice and this permission notice shall be included in +all copies or substantial portions of the Software. +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +THE SOFTWARE + +This file is part of embeddedRTPS. + +Author: i11 - Embedded Software, RWTH Aachen University +*/ + +#include "rtps/messages/MessageReceiver.hpp" +#include + +#include "rtps/entities/Reader.hpp" +#include "rtps/entities/Writer.hpp" +#include "rtps/messages/MessageTypes.hpp" +#include "rtps/utils/Log.hpp" + +#include + +using rtps::MessageReceiver; + +#if RECV_VERBOSE && RTPS_GLOBAL_VERBOSE +#include "rtps/utils/printutils.hpp" +#define RECV_LOG(...) logger_.warn(__VA_ARGS__) +#else +#define RECV_LOG(...) \ + do { \ + } while (0) +#endif + +MessageReceiver::MessageReceiver(Participant *part) + : espp::BaseComponent("RtpsMessageReceiver", espp::Logger::Verbosity::WARN) + , mp_part(part) {} + +bool MessageReceiver::processMessage(const uint8_t *data, DataSize_t size) { + rtps::MessageSourceState sourceState; + MessageProcessingInfo msgInfo(data, size); + + if (!processHeader(msgInfo, sourceState)) { + return false; + } + SubmessageHeader submsgHeader; + while (msgInfo.nextPos < msgInfo.size) { + if (!deserializeMessage(msgInfo, submsgHeader)) { + return false; + } + processSubmessage(msgInfo, submsgHeader, sourceState); + } + + return true; +} + +bool MessageReceiver::processHeader(MessageProcessingInfo &msgInfo, + rtps::MessageSourceState &sourceState) { + Header header; + if (!deserializeMessage(msgInfo, header)) { + return false; + } + + if (header.guidPrefix.id == mp_part->m_guidPrefix.id) { + RECV_LOG("[MessageReceiver]: Received own message."); + return false; // Don't process our own packet + } + + if (header.protocolName != RTPS_PROTOCOL_NAME || + header.protocolVersion.major != PROTOCOLVERSION.major) { + return false; + } + + sourceState.sourceGuidPrefix = header.guidPrefix; + sourceState.sourceVendor = header.vendorId; + sourceState.sourceVersion = header.protocolVersion; + + msgInfo.nextPos += Header::getRawSize(); + return true; +} + +bool MessageReceiver::processSubmessage(MessageProcessingInfo &msgInfo, + const SubmessageHeader &submsgHeader, + const rtps::MessageSourceState &sourceState) { + bool success = false; + + switch (submsgHeader.submessageId) { + case SubmessageKind::ACKNACK: + RECV_LOG("Processing AckNack submessage"); + success = processAckNackSubmessage(msgInfo, sourceState); + break; + case SubmessageKind::DATA: + RECV_LOG("Processing Data submessage"); + success = processDataSubmessage(msgInfo, submsgHeader, sourceState); + break; + case SubmessageKind::HEARTBEAT: + RECV_LOG("Processing Heartbeat submessage"); + success = processHeartbeatSubmessage(msgInfo, sourceState); + break; + case SubmessageKind::INFO_DST: + RECV_LOG("Info_DST submessage not relevant."); + success = true; // Not relevant + break; + case SubmessageKind::GAP: + RECV_LOG("Processing GAP submessage"); + success = processGapSubmessage(msgInfo, sourceState); + break; + case SubmessageKind::INFO_TS: + RECV_LOG("Info_TS submessage not relevant."); + success = true; // Not relevant now + break; + default: + RECV_LOG("Submessage of type {} currently not supported. Skipping..", + static_cast(submsgHeader.submessageId)); + success = false; + } + msgInfo.nextPos += submsgHeader.octetsToNextHeader + SubmessageHeader::getRawSize(); + return success; +} + +bool MessageReceiver::processDataSubmessage(MessageProcessingInfo &msgInfo, + const SubmessageHeader &submsgHeader, + const rtps::MessageSourceState &sourceState) { + SubmessageData dataSubmsg; + if (!deserializeMessage(msgInfo, dataSubmsg)) { + return false; + } + + const uint8_t *submessageStart = msgInfo.getPointerToCurrentPos(); + const uint8_t *submessageEnd = + submessageStart + SubmessageHeader::getRawSize() + submsgHeader.octetsToNextHeader; + const uint16_t submessageBodyOffset = + static_cast(sizeof(dataSubmsg.extraFlags) + sizeof(dataSubmsg.octetsToInlineQos) + + dataSubmsg.octetsToInlineQos); + const uint8_t *serializedData = + submessageStart + SubmessageHeader::getRawSize() + submessageBodyOffset; + + if (serializedData > submessageEnd) { + return false; + } + + if ((submsgHeader.flags & FLAG_INLINE_QOS) != 0) { + const uint8_t *cursor = serializedData; + bool foundSentinel = false; + + while ((cursor + sizeof(uint16_t) + sizeof(uint16_t)) <= submessageEnd) { + uint16_t pid = 0; + uint16_t length = 0; + memcpy(&pid, cursor, sizeof(pid)); + cursor += sizeof(pid); + memcpy(&length, cursor, sizeof(length)); + cursor += sizeof(length); + + if (pid == SMElement::PID_SENTINEL) { + foundSentinel = true; + serializedData = cursor; + break; + } + + if (cursor + length > submessageEnd) { + return false; + } + cursor += length; + + const std::size_t consumed = static_cast(cursor - serializedData); + const std::size_t alignment = (4 - (consumed % 4)) % 4; + if (cursor + alignment > submessageEnd) { + return false; + } + cursor += alignment; + } + + if (!foundSentinel) { + return false; + } + } + + const DataSize_t size = static_cast(submessageEnd - serializedData); + + RECV_LOG("Received data message size {}", static_cast(size)); + + Reader *reader; + if (dataSubmsg.readerId == ENTITYID_UNKNOWN) { +#if RECV_VERBOSE && RTPS_GLOBAL_VERBOSE + RECV_LOG("Received ENTITYID_UNKNOWN readerID, searching for writer ID = "); + printGuid(Guid_t{sourceState.sourceGuidPrefix, dataSubmsg.writerId}); +#endif + reader = + mp_part->getReaderByWriterId(Guid_t{sourceState.sourceGuidPrefix, dataSubmsg.writerId}); + if (reader != nullptr) + RECV_LOG("Found reader!"); + } else { + reader = mp_part->getReader(dataSubmsg.readerId); +#if RECV_VERBOSE && RTPS_GLOBAL_VERBOSE + auto reader_by_writer = + mp_part->getReaderByWriterId(Guid_t{sourceState.sourceGuidPrefix, dataSubmsg.writerId}); + + if (reader_by_writer == nullptr && reader != nullptr) { + RECV_LOG("FOUND By READER ID, NOT BY WRITER ID ="); + printGuid(Guid_t{sourceState.sourceGuidPrefix, dataSubmsg.writerId}); + } +#endif + } + if (reader != nullptr) { + Guid_t writerGuid{sourceState.sourceGuidPrefix, dataSubmsg.writerId}; + ReaderCacheChange change{ChangeKind_t::ALIVE, writerGuid, dataSubmsg.writerSN, serializedData, + size}; + reader->newChange(change); + } else { +#if RECV_VERBOSE && RTPS_GLOBAL_VERBOSE + RECV_LOG("Couldn't find a reader with id: "); + printEntityId(dataSubmsg.readerId); +#endif + } + + return true; +} + +bool MessageReceiver::processHeartbeatSubmessage(MessageProcessingInfo &msgInfo, + const rtps::MessageSourceState &sourceState) { + SubmessageHeartbeat submsgHB; + if (!deserializeMessage(msgInfo, submsgHB)) { + return false; + } + + Reader *reader = mp_part->getReader(submsgHB.readerId); + if (reader != nullptr) { + reader->onNewHeartbeat(submsgHB, sourceState.sourceGuidPrefix); + mp_part->refreshRemoteParticipantLiveliness(sourceState.sourceGuidPrefix); + return true; + } else { + return false; + } +} + +bool MessageReceiver::processAckNackSubmessage(MessageProcessingInfo &msgInfo, + const rtps::MessageSourceState &sourceState) { + SubmessageAckNack submsgAckNack; + if (!deserializeMessage(msgInfo, submsgAckNack)) { + return false; + } + + Writer *writer = mp_part->getWriter(submsgAckNack.writerId); + if (writer != nullptr) { + writer->onNewAckNack(submsgAckNack, sourceState.sourceGuidPrefix); + return true; + } else { + return false; + } +} + +bool MessageReceiver::processGapSubmessage(MessageProcessingInfo &msgInfo, + const rtps::MessageSourceState &sourceState) { + SubmessageGap submsgGap; + if (!deserializeMessage(msgInfo, submsgGap)) { + return false; + } + + Reader *reader = mp_part->getReader(submsgGap.readerId); + if (reader != nullptr) { + reader->onNewGapMessage(submsgGap, sourceState.sourceGuidPrefix); + return true; + } else { + return false; + } +} +#undef RECV_VERBOSE diff --git a/components/rtps_embedded/src/messages/MessageTypes.cpp b/components/rtps_embedded/src/messages/MessageTypes.cpp new file mode 100644 index 000000000..8ed25a2df --- /dev/null +++ b/components/rtps_embedded/src/messages/MessageTypes.cpp @@ -0,0 +1,199 @@ +/* +The MIT License +Copyright (c) 2019 Lehrstuhl Informatik 11 - RWTH Aachen University +Modifications Copyright (c) 2026 ATDev +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: +The above copyright notice and this permission notice shall be included in +all copies or substantial portions of the Software. +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +THE SOFTWARE + +This file is part of embeddedRTPS. + +Author: i11 - Embedded Software, RWTH Aachen University +*/ + +#include "rtps/messages/MessageTypes.hpp" + +#include +#include + +#include +using namespace rtps; + +void doCopyAndMoveOn(uint8_t *dst, const uint8_t *&src, size_t size) { + memcpy(dst, src, size); + src += size; +} + +bool rtps::deserializeMessage(const MessageProcessingInfo &info, Header &header) { + if (info.getRemainingSize() < Header::getRawSize()) { + return false; + } + + const uint8_t *currentPos = info.getPointerToCurrentPos(); + doCopyAndMoveOn(header.protocolName.data(), currentPos, sizeof(std::array)); + doCopyAndMoveOn(reinterpret_cast(&header.protocolVersion), currentPos, + sizeof(ProtocolVersion_t)); + doCopyAndMoveOn(header.vendorId.vendorId.data(), currentPos, header.vendorId.vendorId.size()); + doCopyAndMoveOn(header.guidPrefix.id.data(), currentPos, header.guidPrefix.id.size()); + return true; +} + +bool rtps::deserializeMessage(const MessageProcessingInfo &info, SubmessageHeader &header) { + if (info.getRemainingSize() < SubmessageHeader::getRawSize()) { + return false; + } + + const uint8_t *currentPos = info.getPointerToCurrentPos(); + header.submessageId = static_cast(*currentPos++); + header.flags = *(currentPos++); + doCopyAndMoveOn(reinterpret_cast(&header.octetsToNextHeader), currentPos, + sizeof(uint16_t)); + return true; +} + +bool rtps::deserializeMessage(const MessageProcessingInfo &info, SubmessageData &msg) { + if (info.getRemainingSize() < SubmessageHeader::getRawSize()) { + return false; + } + if (!deserializeMessage(info, msg.header)) { + return false; + } + + // Check for length including data + constexpr auto min_body_size = SubmessageData::getRawSize() - SubmessageHeader::getRawSize(); + if (msg.header.octetsToNextHeader < min_body_size) { + return false; + } + if (info.getRemainingSize() < SubmessageHeader::getRawSize() + msg.header.octetsToNextHeader) { + return false; + } + + const uint8_t *currentPos = info.getPointerToCurrentPos() + SubmessageHeader::getRawSize(); + + doCopyAndMoveOn(reinterpret_cast(&msg.extraFlags), currentPos, sizeof(uint16_t)); + doCopyAndMoveOn(reinterpret_cast(&msg.octetsToInlineQos), currentPos, + sizeof(uint16_t)); + doCopyAndMoveOn(msg.readerId.entityKey.data(), currentPos, msg.readerId.entityKey.size()); + msg.readerId.entityKind = static_cast(*currentPos++); + doCopyAndMoveOn(msg.writerId.entityKey.data(), currentPos, msg.writerId.entityKey.size()); + msg.writerId.entityKind = static_cast(*currentPos++); + doCopyAndMoveOn(reinterpret_cast(&msg.writerSN.high), currentPos, + sizeof(msg.writerSN.high)); + doCopyAndMoveOn(reinterpret_cast(&msg.writerSN.low), currentPos, + sizeof(msg.writerSN.low)); + return true; +} + +bool rtps::deserializeMessage(const MessageProcessingInfo &info, SubmessageHeartbeat &msg) { + if (info.getRemainingSize() < SubmessageHeartbeat::getRawSize()) { + return false; + } + if (!deserializeMessage(info, msg.header)) { + return false; + } + + const uint8_t *currentPos = info.getPointerToCurrentPos() + SubmessageHeader::getRawSize(); + + doCopyAndMoveOn(msg.readerId.entityKey.data(), currentPos, msg.readerId.entityKey.size()); + msg.readerId.entityKind = static_cast(*currentPos++); + doCopyAndMoveOn(msg.writerId.entityKey.data(), currentPos, msg.writerId.entityKey.size()); + msg.writerId.entityKind = static_cast(*currentPos++); + doCopyAndMoveOn(reinterpret_cast(&msg.firstSN.high), currentPos, + sizeof(msg.firstSN.high)); + doCopyAndMoveOn(reinterpret_cast(&msg.firstSN.low), currentPos, + sizeof(msg.firstSN.low)); + doCopyAndMoveOn(reinterpret_cast(&msg.lastSN.high), currentPos, + sizeof(msg.lastSN.high)); + doCopyAndMoveOn(reinterpret_cast(&msg.lastSN.low), currentPos, sizeof(msg.lastSN.low)); + doCopyAndMoveOn(reinterpret_cast(&msg.count.value), currentPos, + sizeof(msg.count.value)); + return true; +} + +void rtps::deserializeSNS(const uint8_t *&position, SequenceNumberSet &set, + std::size_t num_bitfields) { + + doCopyAndMoveOn(reinterpret_cast(&set.base.high), position, + sizeof(SequenceNumber_t::high)); + doCopyAndMoveOn(reinterpret_cast(&set.base.low), position, + sizeof(SequenceNumber_t::low)); + doCopyAndMoveOn(reinterpret_cast(&set.numBits), position, sizeof(uint32_t)); + + // Ensure that we copy not more bits than our sequence number set can hold + if (set.numBits != 0) { + // equal to size = std::min(SNS_NUM_BYTES, num_bitfields) + std::size_t size = num_bitfields > SNS_NUM_BYTES ? SNS_NUM_BYTES : num_bitfields; + doCopyAndMoveOn(reinterpret_cast(set.bitMap.data()), position, size); + position += (num_bitfields - size); + } +} + +bool rtps::deserializeMessage(const MessageProcessingInfo &info, SubmessageAckNack &msg) { + const DataSize_t remainingSizeAtBeginning = info.getRemainingSize(); + if (remainingSizeAtBeginning < + SubmessageAckNack::getRawSizeWithoutSNSet()) { // Size of SequenceNumberSet unknown + return false; + } + if (!deserializeMessage(info, msg.header)) { + return false; + } + + const uint8_t *currentPos = info.getPointerToCurrentPos() + SubmessageHeader::getRawSize(); + + doCopyAndMoveOn(msg.readerId.entityKey.data(), currentPos, msg.readerId.entityKey.size()); + msg.readerId.entityKind = static_cast(*currentPos++); + doCopyAndMoveOn(msg.writerId.entityKey.data(), currentPos, msg.writerId.entityKey.size()); + msg.writerId.entityKind = static_cast(*currentPos++); + + std::size_t num_bitfields = msg.header.octetsToNextHeader - 4 - 4 - 8 - 4 - 4; + deserializeSNS(currentPos, msg.readerSNState, num_bitfields); + + doCopyAndMoveOn(reinterpret_cast(&msg.count.value), currentPos, + sizeof(msg.count.value)); + return true; +} + +bool rtps::deserializeMessage(const MessageProcessingInfo &info, SubmessageGap &msg) { + + const DataSize_t remainingSizeAtBeginning = info.getRemainingSize(); + if (remainingSizeAtBeginning < + SubmessageGap::getRawSizeWithoutSNSet()) { // Size of SequenceNumberSet + // unknown + return false; + } + if (!deserializeMessage(info, msg.header)) { + return false; + } + if (msg.header.octetsToNextHeader < + (SubmessageGap::getRawSizeWithSingleElementSNSet() - SubmessageHeader::getRawSize())) { + return false; + } + const uint8_t *currentPos = info.getPointerToCurrentPos() + SubmessageHeader::getRawSize(); + + doCopyAndMoveOn(msg.readerId.entityKey.data(), currentPos, msg.readerId.entityKey.size()); + msg.readerId.entityKind = static_cast(*currentPos++); + doCopyAndMoveOn(msg.writerId.entityKey.data(), currentPos, msg.writerId.entityKey.size()); + msg.writerId.entityKind = static_cast(*currentPos++); + + doCopyAndMoveOn(reinterpret_cast(&msg.gapStart.high), currentPos, + sizeof(msg.gapStart.high)); + doCopyAndMoveOn(reinterpret_cast(&msg.gapStart.low), currentPos, + sizeof(msg.gapStart.low)); + + std::size_t num_bitfields = msg.header.octetsToNextHeader - 4 - 4 - 8 - 8 - 4; + deserializeSNS(currentPos, msg.gapList, num_bitfields); + + return true; +} diff --git a/components/rtps_embedded/src/utils/Diagnostics.cpp b/components/rtps_embedded/src/utils/Diagnostics.cpp new file mode 100644 index 000000000..84eb2c48b --- /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/pc/CMakeLists.txt b/pc/CMakeLists.txt index a53dea5d2..2d4a3542e 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..22e4b1195 --- /dev/null +++ b/pc/tests/rtps_embedded_publisher.cpp @@ -0,0 +1,82 @@ +// 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 +#include + +#include "espp.hpp" +#include "rtps/entities/Domain.hpp" +#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(), "std_msgs::msg::String", 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 std::string message = std::to_string(value); + const rtps::CacheChange *change = writer->newChange( + rtps::ChangeKind_t::ALIVE, reinterpret_cast(message.c_str()), + static_cast(message.size() + 1)); + 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..b936f2000 --- /dev/null +++ b/pc/tests/rtps_embedded_subscriber.cpp @@ -0,0 +1,131 @@ +// 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.hpp" +#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