From e5bb33c6daaa75cbd35c53a32ca73df5786f0c07 Mon Sep 17 00:00:00 2001 From: William Emfinger Date: Mon, 15 Jun 2026 14:06:23 -0500 Subject: [PATCH 01/24] feat(rtps, cdr): Add `cdr` and `rtps` components --- .github/workflows/build.yml | 4 + .github/workflows/upload_components.yml | 2 + components/cdr/CMakeLists.txt | 3 + components/cdr/README.md | 34 + components/cdr/example/CMakeLists.txt | 21 + components/cdr/example/README.md | 30 + components/cdr/example/main/CMakeLists.txt | 2 + components/cdr/example/main/cdr_example.cpp | 68 + components/cdr/example/sdkconfig.defaults | 1 + components/cdr/idf_component.yml | 21 + components/cdr/include/cdr.hpp | 518 ++++++ components/cdr/src/cdr.cpp | 1 + components/rtps/CMakeLists.txt | 4 + components/rtps/README.md | 60 + components/rtps/example/CMakeLists.txt | 21 + components/rtps/example/README.md | 61 + components/rtps/example/main/CMakeLists.txt | 3 + .../rtps/example/main/Kconfig.projbuild | 85 + components/rtps/example/main/rtps_example.cpp | 221 +++ components/rtps/example/partitions.csv | 5 + components/rtps/example/sdkconfig.defaults | 13 + components/rtps/idf_component.yml | 26 + components/rtps/include/rtps.hpp | 370 +++++ components/rtps/src/rtps.cpp | 1413 +++++++++++++++++ doc/Doxyfile | 4 + doc/conf_common.py | 6 + doc/en/cdr.rst | 32 + doc/en/cdr_example.md | 2 + doc/en/index.rst | 2 + doc/en/rtps.rst | 259 +++ doc/en/rtps_example.md | 2 + doc/en/rtsp.rst | 109 ++ doc/requirements.txt | 1 + docker_build_docs.sh | 2 + python/README.md | 28 +- python/rtps_host.py | 1022 ++++++++++++ 36 files changed, 4455 insertions(+), 1 deletion(-) create mode 100644 components/cdr/CMakeLists.txt create mode 100644 components/cdr/README.md create mode 100644 components/cdr/example/CMakeLists.txt create mode 100644 components/cdr/example/README.md create mode 100644 components/cdr/example/main/CMakeLists.txt create mode 100644 components/cdr/example/main/cdr_example.cpp create mode 100644 components/cdr/example/sdkconfig.defaults create mode 100644 components/cdr/idf_component.yml create mode 100644 components/cdr/include/cdr.hpp create mode 100644 components/cdr/src/cdr.cpp create mode 100644 components/rtps/CMakeLists.txt create mode 100644 components/rtps/README.md create mode 100644 components/rtps/example/CMakeLists.txt create mode 100644 components/rtps/example/README.md create mode 100644 components/rtps/example/main/CMakeLists.txt create mode 100644 components/rtps/example/main/Kconfig.projbuild create mode 100644 components/rtps/example/main/rtps_example.cpp create mode 100644 components/rtps/example/partitions.csv create mode 100644 components/rtps/example/sdkconfig.defaults create mode 100644 components/rtps/idf_component.yml create mode 100644 components/rtps/include/rtps.hpp create mode 100644 components/rtps/src/rtps.cpp create mode 100644 doc/en/cdr.rst create mode 100644 doc/en/cdr_example.md create mode 100644 doc/en/rtps.rst create mode 100644 doc/en/rtps_example.md create mode 100644 python/rtps_host.py diff --git a/.github/workflows/build.yml b/.github/workflows/build.yml index ed5cdb315..37b030deb 100755 --- a/.github/workflows/build.yml +++ b/.github/workflows/build.yml @@ -53,6 +53,8 @@ jobs: target: esp32s3 - path: 'components/chsc6x/example' target: esp32s3 + - path: 'components/cdr/example' + target: esp32 - path: 'components/cli/example' target: esp32 - path: 'components/cobs/example' @@ -177,6 +179,8 @@ jobs: target: esp32s3 - path: 'components/rmt/example' target: esp32s3 + - path: 'components/rtps/example' + target: esp32 - path: 'components/rtsp/example' target: esp32 - path: 'components/runqueue/example' diff --git a/.github/workflows/upload_components.yml b/.github/workflows/upload_components.yml index 9c62f4b5c..b3946eed4 100755 --- a/.github/workflows/upload_components.yml +++ b/.github/workflows/upload_components.yml @@ -47,6 +47,7 @@ jobs: components/button components/byte90 components/chsc6x + components/cdr components/cli components/cobs components/codec @@ -109,6 +110,7 @@ jobs: components/qwiicnes components/remote_debug components/rmt + components/rtps components/rtsp components/runqueue components/rx8130ce diff --git a/components/cdr/CMakeLists.txt b/components/cdr/CMakeLists.txt new file mode 100644 index 000000000..acbe2799e --- /dev/null +++ b/components/cdr/CMakeLists.txt @@ -0,0 +1,3 @@ +idf_component_register( + INCLUDE_DIRS "include" + SRC_DIRS "src") diff --git a/components/cdr/README.md b/components/cdr/README.md new file mode 100644 index 000000000..3a9e941e9 --- /dev/null +++ b/components/cdr/README.md @@ -0,0 +1,34 @@ +# CDR (Common Data Representation) Component + +[![Badge](https://components.espressif.com/components/espp/cdr/badge.svg)](https://components.espressif.com/components/espp/cdr) + +The `cdr` component provides a small, standalone Common Data Representation +(CDR) reader/writer utility aimed at standards-oriented protocols such as +DDS/RTPS. + +This initial slice is intentionally focused on the most immediately useful +pieces for building interoperable payloads: + +- encapsulation identifiers for `CDR_BE`, `CDR_LE`, `PL_CDR_BE`, and `PL_CDR_LE` +- endian-aware primitive read/write helpers +- CDR alignment and padding handling +- string serialization helpers using the standard CDR length-prefix + null terminator format +- headerless/body helpers for CDR fields embedded inside larger protocol elements +- fixed-array helpers and zero-copy payload/span views +- sequence helpers for homogeneous primitive collections +- standalone usage without depending on RTPS or DDS layers + +Current scope: + +- good fit for building RTPS payloads and parameter lists incrementally +- designed to stay reusable outside DDS/RTPS +- **not** yet a full DDS XTypes / XCDR2 implementation + +## Example + +The [example](./example) demonstrates a small round-trip using: + +- a little-endian CDR encapsulation header +- primitive values +- a CDR string +- a `uint16_t` sequence diff --git a/components/cdr/example/CMakeLists.txt b/components/cdr/example/CMakeLists.txt new file mode 100644 index 000000000..491c40697 --- /dev/null +++ b/components/cdr/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 cdr logger" + CACHE STRING + "List of components to include" + ) + +project(cdr_example) + +set(CMAKE_CXX_STANDARD 20) diff --git a/components/cdr/example/README.md b/components/cdr/example/README.md new file mode 100644 index 000000000..85b9fddb0 --- /dev/null +++ b/components/cdr/example/README.md @@ -0,0 +1,30 @@ +# CDR Example + +This example demonstrates a small CDR round-trip using the `cdr` component. + +It exercises: + +- a little-endian CDR encapsulation header +- primitive value serialization +- CDR string serialization +- `uint16_t` sequence serialization +- fixed-array serialization with `write_array` / `read_array` +- headerless/body CDR helpers for embedding fields inside a larger protocol value +- round-trip parsing with `CdrReader` + +## How to use example + +### Build and Flash + +Build the project and flash it to the board, then run monitor tool to view serial output: + +```bash +idf.py -p PORT flash monitor +``` + +Replace `PORT` with the name of the serial port to use. + +## Expected Output + +The example logs the encoded byte count and the decoded values. It finishes by +printing `CDR round-trip succeeded`. diff --git a/components/cdr/example/main/CMakeLists.txt b/components/cdr/example/main/CMakeLists.txt new file mode 100644 index 000000000..a941e22ba --- /dev/null +++ b/components/cdr/example/main/CMakeLists.txt @@ -0,0 +1,2 @@ +idf_component_register(SRC_DIRS "." + INCLUDE_DIRS ".") diff --git a/components/cdr/example/main/cdr_example.cpp b/components/cdr/example/main/cdr_example.cpp new file mode 100644 index 000000000..6bc8662cb --- /dev/null +++ b/components/cdr/example/main/cdr_example.cpp @@ -0,0 +1,68 @@ +#include +#include +#include +#include + +#include "cdr.hpp" +#include "logger.hpp" + +extern "C" void app_main(void) { + espp::Logger logger({.tag = "cdr_example", .level = espp::Logger::Verbosity::INFO}); + + std::array input_magic{'C', 'D', 'R', '!'}; + std::array input_values{10, 20, 30}; + + espp::CdrWriter writer({ + .encapsulation = espp::CdrEncapsulation::CDR_LE, + .include_encapsulation = true, + }); + + // cdr example + writer.write(42); + writer.write(3.25f); + writer.write_string("hello cdr"); + writer.write_sequence(input_values); + + auto payload = writer.take_buffer(); + logger.info("Serialized {} bytes of CDR data", payload.size()); + + auto inline_writer = espp::CdrWriter::make_body_writer(espp::CdrEncapsulation::CDR_LE); + inline_writer.write_array(input_magic); + inline_writer.write_string("embedded field"); + auto inline_payload = + espp::CdrWriter::encapsulate(inline_writer.payload(), espp::CdrEncapsulation::PL_CDR_LE); + + espp::CdrReader reader(payload); + espp::CdrReader inline_reader(inline_payload); + uint32_t decoded_count = 0; + float decoded_scale = 0.0f; + std::string decoded_text; + std::vector decoded_values; + std::array decoded_magic{}; + std::string decoded_inline_text; + + bool ok = reader.read(decoded_count) && reader.read(decoded_scale) && + reader.read_string(decoded_text) && reader.read_sequence(decoded_values); + bool inline_ok = inline_reader.encapsulation() == espp::CdrEncapsulation::PL_CDR_LE && + inline_reader.read_array(decoded_magic) && + inline_reader.read_string(decoded_inline_text); + + if (!ok || !inline_ok) { + logger.error("Failed to decode CDR payload"); + return; + } + + logger.info("Decoded count={}, scale={:.2f}, text='{}', sequence size={}, embedded='{}'", + decoded_count, decoded_scale, decoded_text, decoded_values.size(), + decoded_inline_text); + + if (decoded_count != 42 || decoded_scale != 3.25f || decoded_text != "hello cdr" || + decoded_values.size() != input_values.size() || + !std::equal(decoded_values.begin(), decoded_values.end(), input_values.begin()) || + decoded_magic != input_magic || decoded_inline_text != "embedded field") { + logger.error("CDR round-trip mismatch"); + return; + } + + logger.info("CDR round-trip succeeded"); +} diff --git a/components/cdr/example/sdkconfig.defaults b/components/cdr/example/sdkconfig.defaults new file mode 100644 index 000000000..8a325dde2 --- /dev/null +++ b/components/cdr/example/sdkconfig.defaults @@ -0,0 +1 @@ +# Intentionally minimal; this example only exercises the CDR helpers. diff --git a/components/cdr/idf_component.yml b/components/cdr/idf_component.yml new file mode 100644 index 000000000..92d805db4 --- /dev/null +++ b/components/cdr/idf_component.yml @@ -0,0 +1,21 @@ +## IDF Component Manager Manifest File +license: "MIT" +description: "Common Data Representation (CDR) read/write helpers for ESP-IDF and cross-platform use." +url: "https://github.com/esp-cpp/espp/tree/main/components/cdr" +repository: "git://github.com/esp-cpp/espp.git" +maintainers: + - William Emfinger +documentation: "https://esp-cpp.github.io/espp/cdr.html" +examples: + - path: example +tags: + - cpp + - Component + - CDR + - DDS + - RTPS + - Serialization + - XCDR +dependencies: + idf: + version: '>=5.0' diff --git a/components/cdr/include/cdr.hpp b/components/cdr/include/cdr.hpp new file mode 100644 index 000000000..a3c8a5944 --- /dev/null +++ b/components/cdr/include/cdr.hpp @@ -0,0 +1,518 @@ +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace espp { + +/// Supported CDR encapsulation identifiers. +enum class CdrEncapsulation : uint16_t { + CDR_BE = 0x0000, ///< Big-endian Common Data Representation. + CDR_LE = 0x0001, ///< Little-endian Common Data Representation. + PL_CDR_BE = 0x0002, ///< Big-endian parameter-list CDR. + PL_CDR_LE = 0x0003, ///< Little-endian parameter-list CDR. +}; + +namespace detail { +template inline T swap_endian(T value) { + auto bytes = std::bit_cast>(value); + std::reverse(bytes.begin(), bytes.end()); + return std::bit_cast(bytes); +} + +template inline T convert_endian(T value, bool target_little_endian) { + if constexpr (sizeof(T) == 1) { + return value; + } else { + if ((std::endian::native == std::endian::little) == target_little_endian) { + return value; + } + return swap_endian(value); + } +} + +constexpr size_t cdr_alignment_for_size(size_t size) { + return size >= 8 ? 8 : (size >= 4 ? 4 : (size >= 2 ? 2 : 1)); +} + +template constexpr size_t cdr_alignment() { return cdr_alignment_for_size(sizeof(T)); } +} // namespace detail + +/// Small helper for building CDR/XCDR1-style byte streams. +/// +/// \section cdr_ex1 CDR Example +/// \snippet cdr_example.cpp cdr example +class CdrWriter { +public: + /// @brief Configuration for a CDR writer instance. + struct Config { + CdrEncapsulation encapsulation{ + CdrEncapsulation::CDR_LE}; ///< Encapsulation kind to emit when writing. + bool include_encapsulation{ + true}; ///< If true, prepend the 4-byte encapsulation header to the buffer. + }; + + /// @brief Create a configuration for writing a CDR body without an encapsulation header. + /// @param encapsulation Endianness/encapsulation rules to use for the body payload. + /// @return A configuration with encapsulation emission disabled. + [[nodiscard]] static Config + body_config(CdrEncapsulation encapsulation = CdrEncapsulation::CDR_LE) { + return { + .encapsulation = encapsulation, + .include_encapsulation = false, + }; + } + + /// @brief Create a writer configured for a headerless/body-only CDR payload. + /// @param encapsulation Endianness/encapsulation rules to use for the body payload. + /// @return A ready-to-use writer with no encapsulation header in its output. + [[nodiscard]] static CdrWriter + make_body_writer(CdrEncapsulation encapsulation = CdrEncapsulation::CDR_LE) { + return CdrWriter(body_config(encapsulation)); + } + + /// @brief Wrap an existing payload with a CDR encapsulation header. + /// @param payload Raw bytes to append after the generated encapsulation header. + /// @param encapsulation Encapsulation header to prepend. + /// @return A new byte buffer containing the encapsulation header followed by the payload. + [[nodiscard]] static std::vector + encapsulate(std::span payload, + CdrEncapsulation encapsulation = CdrEncapsulation::CDR_LE) { + CdrWriter writer({ + .encapsulation = encapsulation, + .include_encapsulation = true, + }); + writer.write_bytes(payload); + return writer.take_buffer(); + } + + /// @brief Construct a writer using the default little-endian encapsulated configuration. + CdrWriter() { reset(); } + + /// @brief Construct a writer using an explicit configuration. + /// @param config Writer configuration controlling encapsulation and endianness behavior. + explicit CdrWriter(const Config &config) + : config_(config) { + reset(); + } + + /// @brief Clear the current buffer and reinitialize the encapsulation header if configured. + void reset() { + data_.clear(); + if (config_.include_encapsulation) { + auto value = static_cast(config_.encapsulation); + data_.push_back(static_cast((value >> 8) & 0xff)); + data_.push_back(static_cast(value & 0xff)); + data_.push_back(0); + data_.push_back(0); + } + } + + /// @brief Get the configured encapsulation kind for this writer. + /// @return The encapsulation kind associated with this writer. + [[nodiscard]] CdrEncapsulation encapsulation() const { return config_.encapsulation; } + + /// @brief Determine whether values are encoded in little-endian order. + /// @return True for little-endian encapsulations, false for big-endian ones. + [[nodiscard]] bool uses_little_endian() const { + return config_.encapsulation == CdrEncapsulation::CDR_LE || + config_.encapsulation == CdrEncapsulation::PL_CDR_LE; + } + + /// @brief Get the total number of bytes currently written. + /// @return The size of the backing byte buffer, including any encapsulation header. + [[nodiscard]] size_t size() const { return data_.size(); } + + /// @brief Access the full serialized buffer built so far. + /// @return A const reference to the complete buffer, including any encapsulation header. + [[nodiscard]] const std::vector &buffer() const { return data_; } + + /// @brief Access only the payload portion of the buffer. + /// @return A view over the serialized bytes after any encapsulation header. + [[nodiscard]] std::span payload() const { + auto bytes = std::span{data_.data(), data_.size()}; + return bytes.subspan(std::min(payload_offset(), bytes.size())); + } + + /// @brief Move the complete serialized buffer out of the writer. + /// @return The current buffer contents, including any encapsulation header. + [[nodiscard]] std::vector take_buffer() { return std::move(data_); } + + /// @brief Pad the buffer with zeros until it satisfies the requested alignment. + /// @param alignment Required alignment in bytes. Values less than or equal to 1 are ignored. + void align(size_t alignment) { + if (alignment <= 1) { + return; + } + while (data_.size() % alignment != 0) { + data_.push_back(0); + } + } + + /// @brief Append a primitive scalar using CDR alignment and endianness rules. + /// @tparam T Primitive integral or floating-point type to encode. + /// @param value Value to append to the serialized buffer. + /// @return True after the value has been encoded and appended. + template + requires(std::is_integral_v || std::is_floating_point_v) bool write(T value) { + align(detail::cdr_alignment()); + auto encoded = detail::convert_endian(value, uses_little_endian()); + auto bytes = std::bit_cast>(encoded); + auto *raw = reinterpret_cast(bytes.data()); + data_.insert(data_.end(), raw, raw + bytes.size()); + return true; + } + + /// @brief Append a boolean value using the standard CDR 1-byte representation. + /// @param value Boolean value to encode. + /// @return True after the value has been appended. + bool write_bool(bool value) { return write(value ? 1 : 0); } + + /// @brief Append a CDR string. + /// @param text UTF-8 text to encode. A terminating null byte is written automatically. + /// @return True after the string length, contents, terminator, and alignment padding are written. + bool write_string(std::string_view text) { + align(4); + write(static_cast(text.size() + 1)); + data_.insert(data_.end(), text.begin(), text.end()); + data_.push_back(0); + align(4); + return true; + } + + /// @brief Append raw bytes with optional alignment. + /// @param bytes Bytes to copy into the serialized buffer. + /// @param alignment Alignment in bytes to satisfy before appending the data. + /// @return True after the bytes have been appended. + bool write_bytes(std::span bytes, size_t alignment = 1) { + align(alignment); + data_.insert(data_.end(), bytes.begin(), bytes.end()); + return true; + } + + /// @brief Append a fixed-size array of primitive values. + /// @tparam T Primitive integral or floating-point element type. + /// @tparam N Number of elements in the array. + /// @param values Array to encode element-by-element. + /// @return True after all elements have been encoded. + template + requires(std::is_integral_v || + std::is_floating_point_v) bool write_array(const std::array &values) { + for (const auto &value : values) { + if (!write(value)) { + return false; + } + } + return true; + } + + /// @brief Append a variable-length CDR sequence of primitive values. + /// @tparam T Primitive integral or floating-point element type. + /// @param values Sequence elements to encode. + /// @return True after the sequence length and all elements have been encoded. + template + requires(std::is_integral_v || + std::is_floating_point_v) bool write_sequence(std::span values) { + align(4); + write(static_cast(values.size())); + for (const auto &value : values) { + write(value); + } + return true; + } + +private: + [[nodiscard]] size_t payload_offset() const { return config_.include_encapsulation ? 4 : 0; } + + Config config_; + std::vector data_{}; +}; + +/// Small helper for parsing CDR/XCDR1-style byte streams. +class CdrReader { +public: + /// @brief Configuration for a CDR reader instance. + struct Config { + bool expect_encapsulation{ + true}; ///< If true, consume and validate a 4-byte encapsulation header on reset. + CdrEncapsulation default_encapsulation{ + CdrEncapsulation::CDR_LE}; ///< Encapsulation to assume when no header is expected. + }; + + /// @brief Create a configuration for reading a headerless/body-only CDR payload. + /// @param encapsulation Encapsulation/endian rules to assume for the payload body. + /// @return A configuration with encapsulation consumption disabled. + [[nodiscard]] static Config + body_config(CdrEncapsulation encapsulation = CdrEncapsulation::CDR_LE) { + return { + .expect_encapsulation = false, + .default_encapsulation = encapsulation, + }; + } + + /// @brief Create a reader for a headerless/body-only CDR payload. + /// @param data Serialized payload bytes to read. + /// @param encapsulation Encapsulation/endian rules to assume for the payload body. + /// @return A reader initialized for body-only parsing. + [[nodiscard]] static CdrReader + make_body_reader(std::span data, + CdrEncapsulation encapsulation = CdrEncapsulation::CDR_LE) { + return CdrReader(data, body_config(encapsulation)); + } + + /// @brief Construct a reader that expects a standard encapsulated CDR payload. + /// @param data Serialized bytes to parse. + explicit CdrReader(std::span data) { reset(data); } + + /// @brief Construct a reader with an explicit configuration. + /// @param data Serialized bytes to parse. + /// @param config Reader configuration controlling encapsulation handling. + CdrReader(std::span data, const Config &config) + : config_(config) { + reset(data); + } + + /// @brief Reset the reader to the beginning of a new serialized buffer. + /// @param data Serialized bytes to parse. + void reset(std::span data) { + data_ = data; + offset_ = 0; + valid_ = true; + if (config_.expect_encapsulation) { + if (data_.size() < 4) { + valid_ = false; + return; + } + uint16_t raw = static_cast(data_[0] << 8) | static_cast(data_[1]); + switch (raw) { + case static_cast(CdrEncapsulation::CDR_BE): + case static_cast(CdrEncapsulation::CDR_LE): + case static_cast(CdrEncapsulation::PL_CDR_BE): + case static_cast(CdrEncapsulation::PL_CDR_LE): + encapsulation_ = static_cast(raw); + break; + default: + valid_ = false; + return; + } + offset_ = 4; + } else { + encapsulation_ = config_.default_encapsulation; + } + } + + /// @brief Check whether the reader is still in a valid state. + /// @return True if parsing can continue, false if a prior operation failed. + [[nodiscard]] bool valid() const { return valid_; } + + /// @brief Get the active encapsulation for the current buffer. + /// @return The parsed or assumed encapsulation value. + [[nodiscard]] CdrEncapsulation encapsulation() const { return encapsulation_; } + + /// @brief Determine whether values are decoded as little-endian. + /// @return True for little-endian encapsulations, false for big-endian ones. + [[nodiscard]] bool uses_little_endian() const { + return encapsulation_ == CdrEncapsulation::CDR_LE || + encapsulation_ == CdrEncapsulation::PL_CDR_LE; + } + + /// @brief Access only the payload bytes after any encapsulation header. + /// @return A view over the unread buffer excluding the encapsulation header. + [[nodiscard]] std::span payload() const { + return data_.subspan(std::min(payload_offset(), data_.size())); + } + + /// @brief Get the number of unread bytes remaining. + /// @return Remaining unread byte count, or 0 if the offset is beyond the buffer. + [[nodiscard]] size_t remaining() const { + return offset_ <= data_.size() ? data_.size() - offset_ : 0; + } + + /// @brief Access a view of the unread bytes without copying. + /// @return A span over the unread tail of the buffer, or an empty span if the reader is invalid. + [[nodiscard]] std::span remaining_view() const { + if (!valid_) { + return {}; + } + return data_.subspan(std::min(offset_, data_.size())); + } + + /// @brief Advance the read cursor by a fixed number of bytes. + /// @param length Number of bytes to skip. + /// @return True if the bytes were skipped, false if the reader became invalid. + bool skip(size_t length) { + if (!valid_ || remaining() < length) { + valid_ = false; + return false; + } + offset_ += length; + return true; + } + + /// @brief Advance the read cursor to satisfy an alignment requirement. + /// @param alignment Required alignment in bytes. Values less than or equal to 1 are ignored. + /// @return True if the padding bytes were skipped successfully, false if the reader became + /// invalid. + bool align(size_t alignment) { + if (alignment <= 1) { + return true; + } + size_t padding = (alignment - (offset_ % alignment)) % alignment; + return skip(padding); + } + + /// @brief Read a primitive scalar using CDR alignment and endianness rules. + /// @tparam T Primitive integral or floating-point type to decode. + /// @param value Output variable that receives the decoded value on success. + /// @return True if a complete value was decoded, false otherwise. + template + requires(std::is_integral_v || std::is_floating_point_v) bool read(T &value) { + if (!align(detail::cdr_alignment()) || remaining() < sizeof(T)) { + valid_ = false; + return false; + } + std::array bytes{}; + std::memcpy(bytes.data(), data_.data() + offset_, sizeof(T)); + offset_ += sizeof(T); + auto encoded = std::bit_cast(bytes); + value = detail::convert_endian(encoded, uses_little_endian()); + return true; + } + + /// @brief Read a boolean value encoded with the CDR 1-byte representation. + /// @param value Output variable that receives the decoded boolean on success. + /// @return True if the boolean was read successfully, false otherwise. + bool read_bool(bool &value) { + uint8_t raw = 0; + if (!read(raw)) { + return false; + } + value = raw != 0; + return true; + } + + /// @brief Read a CDR string. + /// @param text Output string receiving the decoded text without the trailing null terminator. + /// @return True if the string was decoded successfully, false otherwise. + bool read_string(std::string &text) { + if (!align(4)) { + return false; + } + uint32_t length = 0; + if (!read(length) || length == 0 || remaining() < length) { + valid_ = false; + return false; + } + auto span = data_.subspan(offset_, length); + offset_ += length; + if (span.back() == 0) { + span = span.first(span.size() - 1); + } + text.assign(reinterpret_cast(span.data()), span.size()); + return align(4); + } + + /// @brief Read a fixed number of bytes as a zero-copy span. + /// @param length Number of bytes to expose. + /// @param alignment Alignment in bytes to satisfy before reading. + /// @return A span over the requested bytes, or an empty span if the read failed. + std::span read_span(size_t length, size_t alignment = 1) { + if (!align(alignment) || remaining() < length) { + valid_ = false; + return {}; + } + auto span = data_.subspan(offset_, length); + offset_ += length; + return span; + } + + /// @brief Read bytes into a caller-provided mutable span. + /// @param bytes Destination span that receives the copied bytes. + /// @param alignment Alignment in bytes to satisfy before reading. + /// @return True if all requested bytes were read successfully, false otherwise. + bool read_bytes(std::span bytes, size_t alignment = 1) { + auto span = read_span(bytes.size(), alignment); + if (span.size() != bytes.size()) { + return false; + } + std::memcpy(bytes.data(), span.data(), bytes.size()); + return true; + } + + /// @brief Read bytes into a vector. + /// @param bytes Output vector replaced with the decoded bytes on success. + /// @param length Number of bytes to read. + /// @param alignment Alignment in bytes to satisfy before reading. + /// @return True if the requested bytes were read successfully, false otherwise. + bool read_bytes(std::vector &bytes, size_t length, size_t alignment = 1) { + auto span = read_span(length, alignment); + if (span.size() != length) { + return false; + } + bytes.assign(span.begin(), span.end()); + return true; + } + + /// @brief Read a fixed-size array of primitive values. + /// @tparam T Primitive integral or floating-point element type. + /// @tparam N Number of elements in the array. + /// @param values Output array receiving the decoded elements. + /// @return True if all array elements were read successfully, false otherwise. + template + requires(std::is_integral_v || + std::is_floating_point_v) bool read_array(std::array &values) { + for (auto &value : values) { + if (!read(value)) { + return false; + } + } + return true; + } + + /// @brief Read a variable-length CDR sequence of primitive values. + /// @tparam T Primitive integral or floating-point element type. + /// @param values Output vector replaced with the decoded sequence elements on success. + /// @return True if the sequence length and all elements were decoded successfully, false + /// otherwise. + template + requires(std::is_integral_v || + std::is_floating_point_v) bool read_sequence(std::vector &values) { + if (!align(4)) { + return false; + } + uint32_t length = 0; + if (!read(length)) { + return false; + } + values.clear(); + values.reserve(length); + for (uint32_t i = 0; i < length; i++) { + T value{}; + if (!read(value)) { + return false; + } + values.push_back(value); + } + return true; + } + +private: + [[nodiscard]] size_t payload_offset() const { return config_.expect_encapsulation ? 4 : 0; } + + Config config_{}; + std::span data_{}; + size_t offset_{0}; + bool valid_{false}; + CdrEncapsulation encapsulation_{CdrEncapsulation::CDR_LE}; +}; + +} // namespace espp diff --git a/components/cdr/src/cdr.cpp b/components/cdr/src/cdr.cpp new file mode 100644 index 000000000..584ad00cc --- /dev/null +++ b/components/cdr/src/cdr.cpp @@ -0,0 +1 @@ +#include "cdr.hpp" diff --git a/components/rtps/CMakeLists.txt b/components/rtps/CMakeLists.txt new file mode 100644 index 000000000..1dc01861d --- /dev/null +++ b/components/rtps/CMakeLists.txt @@ -0,0 +1,4 @@ +idf_component_register( + INCLUDE_DIRS "include" + SRC_DIRS "src" + REQUIRES base_component cdr task socket) diff --git a/components/rtps/README.md b/components/rtps/README.md new file mode 100644 index 000000000..05ec19090 --- /dev/null +++ b/components/rtps/README.md @@ -0,0 +1,60 @@ +# RTPS Component + +[![Badge](https://components.espressif.com/components/espp/rtps/badge.svg)](https://components.espressif.com/components/espp/rtps) + +The `RtpsParticipant` component is the beginning of a cross-platform RTPS +(Real-Time Publish-Subscribe) implementation built on top of the ESPP `socket` +component. + +This component now includes the first real RTPS discovery slice: + +- RTPS header and DATA submessage framing helpers +- standard RTPS UDPv4 port calculations +- GUID, entity ID, locator, and sequence number utility types +- SPDP participant announcements using PL_CDR parameter lists +- SEDP publication and subscription announcements for local endpoints +- parsing and tracking of discovered remote participants, writers, and readers +- integration with the shared `cdr` component for CDR/PL_CDR payload handling + +The long-term goal for this component is DDS/RTPS interoperability with ROS 2 +nodes, including best-effort and reliable user-data flows. Discovery is now +standards-shaped, but the reliable RTPS state machines (`HEARTBEAT`, +`ACKNACK`, resend windows) and ROS 2 endpoint/user-data interoperability are +still incomplete. + +## Expected Compatibility + +The table below is intentionally conservative: **expected** means "this is the +intended scope based on the current wire format and code", not "fully verified +against every stack". + +| Peer implementation | Expected compatibility | Notes | +| --- | --- | --- | +| ESPP `rtps` component / `python/rtps_host.py` | **Yes** for current scaffold | Intended smoke-test path for SPDP, SEDP, and the temporary `UInt32` `ESPPDATA` user-data payload. | +| Generic DDSI-RTPS 2.3 implementations | **Partial** | SPDP and SEDP messages are standards-shaped, but only the discovery slice is implemented today. | +| ROS 2 nodes backed by Fast DDS | **Partial / discovery-targeted** | The current discovery messages include ROS 2-relevant participant user data such as `enclave=...;`, but standards-based ROS 2 topic data exchange is not finished yet. | +| ROS 2 nodes backed by Cyclone DDS or other DDS vendors | **Partial / unverified** | Expected to be limited to the minimal discovery subset if the peer accepts the currently emitted parameter set; not validated yet. | +| Reliable DDS/RTPS endpoints | **No** | `HEARTBEAT`, `ACKNACK`, retransmission windows, and other reliable state-machine pieces are not implemented. | + +## Feature Status + +| Feature | Status | Notes | +| --- | --- | --- | +| RTPS header / DATA submessage serialize + parse | **Implemented** | Core message framing is present. | +| Standard UDPv4 RTPS port mapping | **Implemented** | Uses the DDSI-RTPS well-known port formula. | +| SPDP participant announce send/receive | **Implemented** | Multicast announce plus participant cache updates. | +| SEDP publication / subscription announce send/receive | **Implemented** | Local endpoints are announced and remote endpoints are cached. | +| Participant / endpoint discovery callbacks | **Implemented** | Exposed through `on_participant_discovered` and `on_endpoint_discovered`. | +| Temporary `UInt32` user-data path | **Implemented** | Uses the current ESPP-specific `ESPPDATA` payload, not a standards-based DDS sample representation. | +| QoS fields emitted in discovery | **Partial** | Reliability, durability, liveliness, and history parameters are advertised in SEDP. | +| QoS matching / policy enforcement | **Not implemented** | Remote QoS is parsed, but full writer/reader matching logic is still missing. | +| Standards-based DDS user-data serialization | **Not implemented** | The current data path is a temporary ESPP scaffold for `std_msgs/msg/UInt32`. | +| Inline QoS handling | **Not implemented** | Discovery and user-data handling assume no inline QoS. | +| Reliable RTPS (`HEARTBEAT`, `ACKNACK`, resend`) | **Not implemented** | Reliable delivery is not interoperable yet. | +| Full ROS 2 topic interoperability | **Not implemented** | Discovery is the current milestone; ROS 2-compatible data writers/readers are still pending. | + +## Example + +The [example](./example) exercises the protocol helpers, computes the standard +RTPS ports, builds/parses SPDP and SEDP messages, and demonstrates the +participant API without requiring a second device. diff --git a/components/rtps/example/CMakeLists.txt b/components/rtps/example/CMakeLists.txt new file mode 100644 index 000000000..e64b1c087 --- /dev/null +++ b/components/rtps/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 wifi" + CACHE STRING + "List of components to include" + ) + +project(rtps_example) + +set(CMAKE_CXX_STANDARD 20) diff --git a/components/rtps/example/README.md b/components/rtps/example/README.md new file mode 100644 index 000000000..11f5259ee --- /dev/null +++ b/components/rtps/example/README.md @@ -0,0 +1,61 @@ +# RTPS Example + +This example now acts as a two-node RTPS smoke test for ESP targets on the same +Wi-Fi network. + +It demonstrates: + +* Wi-Fi STA setup for host-network RTPS traffic +* standard RTPS UDP port calculation for each participant +* SPDP participant discovery between two boards +* SEDP endpoint discovery for request/response topics +* CDR little-endian serialization for `std_msgs/msg/UInt32`-style payloads +* best-effort inter-node request/response sample exchange + +The component's long-term goal is ROS 2 interoperability over DDS/RTPS. This +example focuses on proving cross-board discovery and user-data delivery using +the current scaffold. + +## How to use example + +### Configure two boards + +Build one board as the **initiator** and the other as the **responder**. + +For both boards: + +1. Set the same `RTPS domain ID`, `Topic prefix`, `WiFi SSID`, and `WiFi password`. +2. Give each board a unique `RTPS participant ID`. +3. Optionally set distinct `Participant node name` values to make discovery logs easier to read. + +For one board only: + +1. Select `RTPS Example Configuration -> Example Role -> Initiator` + +For the other board: + +1. Select `RTPS Example Configuration -> Example Role -> Responder` + +### Build and Flash + +Build the project and flash it to the board, then run monitor tool to view +serial output: + +```sh +idf.py -p PORT flash monitor +``` + +(Replace PORT with the name of the serial port to use.) + +## Example Output + +The initiator waits until it discovers the responder's endpoints, then publishes +incrementing values on `/request`. The responder logs each +received request and echoes the same value back on `/response`. + +Expected signs of success: + +* both boards report Wi-Fi connection and their local IP address +* both boards log RTPS participant and endpoint discovery +* the initiator logs `Published request N` followed by `Received response N` +* the responder logs `Received request N, sending response` diff --git a/components/rtps/example/main/CMakeLists.txt b/components/rtps/example/main/CMakeLists.txt new file mode 100644 index 000000000..e398cc2f0 --- /dev/null +++ b/components/rtps/example/main/CMakeLists.txt @@ -0,0 +1,3 @@ +idf_component_register(SRC_DIRS "." + INCLUDE_DIRS "." + REQUIRES logger rtps wifi) diff --git a/components/rtps/example/main/Kconfig.projbuild b/components/rtps/example/main/Kconfig.projbuild new file mode 100644 index 000000000..ff2656842 --- /dev/null +++ b/components/rtps/example/main/Kconfig.projbuild @@ -0,0 +1,85 @@ +menu "RTPS Example Configuration" + + choice RTPS_EXAMPLE_ROLE + prompt "Example Role" + default RTPS_EXAMPLE_ROLE_INITIATOR + help + Build one board as the initiator and a second board as the responder + to verify RTPS discovery and end-to-end UInt32 sample exchange. + + config RTPS_EXAMPLE_ROLE_INITIATOR + bool "Initiator" + help + Publishes incrementing request samples after it discovers a + responder, then logs the matching responses. + + config RTPS_EXAMPLE_ROLE_RESPONDER + bool "Responder" + help + Waits for request samples and echoes the same value back on the + response topic. + + endchoice + + config RTPS_EXAMPLE_NODE_NAME + string "Participant node name" + default "espp_rtps_node" + help + Logical RTPS participant name announced during discovery. + + config RTPS_EXAMPLE_DOMAIN_ID + int "RTPS domain ID" + range 0 232 + default 0 + help + Both boards must use the same domain ID to discover each other. + + config RTPS_EXAMPLE_PARTICIPANT_ID + int "RTPS participant ID" + range 0 119 + default 1 + help + Each board should use a unique participant ID within the same domain. + + config RTPS_EXAMPLE_TOPIC_PREFIX + string "Topic prefix" + default "espp/rtps_example" + help + Prefix used to derive the request and response topics. + + config RTPS_EXAMPLE_ANNOUNCE_PERIOD_MS + int "Discovery announce period (ms)" + range 200 10000 + default 1500 + help + Period between periodic SPDP/SEDP discovery announcements. + + config RTPS_EXAMPLE_PUBLISH_PERIOD_MS + int "Initiator publish period (ms)" + range 250 60000 + default 2000 + depends on RTPS_EXAMPLE_ROLE_INITIATOR + help + Period between request messages sent by the initiator after a + responder has been discovered. + + config ESP_WIFI_SSID + string "WiFi SSID" + default "" + help + SSID (network name) for the example to connect to. + + config ESP_WIFI_PASSWORD + string "WiFi Password" + default "" + help + WiFi password (WPA or WPA2) for the example to use. + + config ESP_MAXIMUM_RETRY + int "Maximum retry" + default 5 + help + Set the maximum retry count to avoid reconnecting forever when the + network is unavailable. + +endmenu diff --git a/components/rtps/example/main/rtps_example.cpp b/components/rtps/example/main/rtps_example.cpp new file mode 100644 index 000000000..2abce874f --- /dev/null +++ b/components/rtps/example/main/rtps_example.cpp @@ -0,0 +1,221 @@ +#include +#include +#include +#include + +#include "logger.hpp" +#include "rtps.hpp" +#include "wifi_sta.hpp" + +using namespace std::chrono_literals; + +namespace { +constexpr std::string_view kTypeName = "std_msgs/msg/UInt32"; + +bool run_local_protocol_checks(espp::Logger &logger, const espp::RtpsParticipant &participant) { + auto announce_message = participant.build_announce_message(); + auto parsed_message = espp::RtpsParticipant::Message::parse(announce_message); + if (!parsed_message) { + logger.error("Failed to parse locally built announce message"); + return false; + } + logger.info("Built and parsed SPDP announce message with {} submessage(s)", + parsed_message->submessages.size()); + + if (!participant.writers().empty()) { + auto sedp_publication_message = + participant.build_sedp_publication_message(participant.writers().front()); + auto parsed_publication_message = + espp::RtpsParticipant::Message::parse(sedp_publication_message); + if (!parsed_publication_message) { + logger.error("Failed to parse locally built SEDP publication message"); + return false; + } + logger.info("Built and parsed SEDP publication message with {} submessage(s)", + parsed_publication_message->submessages.size()); + } + + if (!participant.readers().empty()) { + auto sedp_subscription_message = + participant.build_sedp_subscription_message(participant.readers().front()); + auto parsed_subscription_message = + espp::RtpsParticipant::Message::parse(sedp_subscription_message); + if (!parsed_subscription_message) { + logger.error("Failed to parse locally built SEDP subscription message"); + return false; + } + logger.info("Built and parsed SEDP subscription message with {} submessage(s)", + parsed_subscription_message->submessages.size()); + } + + auto uint32_payload = espp::RtpsParticipant::serialize_uint32_cdr(42); + auto maybe_value = espp::RtpsParticipant::deserialize_uint32_cdr(uint32_payload); + if (!maybe_value || *maybe_value != 42) { + logger.error("UInt32 CDR round trip failed"); + return false; + } + logger.info("UInt32 CDR round trip succeeded with value {}", *maybe_value); + return true; +} + +bool has_endpoint(std::span endpoints, + std::string_view topic_name, bool is_reader) { + return std::any_of(endpoints.begin(), endpoints.end(), + [topic_name, is_reader](const auto &endpoint) { + return endpoint.topic_name == topic_name && endpoint.is_reader == is_reader; + }); +} +} // namespace + +extern "C" void app_main(void) { + espp::Logger logger({.tag = "rtps_example", .level = espp::Logger::Verbosity::INFO}); + + std::string ip_address; + espp::WifiSta wifi_sta({.ssid = CONFIG_ESP_WIFI_SSID, + .password = CONFIG_ESP_WIFI_PASSWORD, + .num_connect_retries = CONFIG_ESP_MAXIMUM_RETRY, + .on_connected = nullptr, + .on_disconnected = nullptr, + .on_got_ip = [&ip_address](ip_event_got_ip_t *eventdata) { + ip_address = fmt::format("{}.{}.{}.{}", IP2STR(&eventdata->ip_info.ip)); + fmt::print("got IP: {}\n", ip_address); + }}); + + logger.info("Waiting for WiFi connection..."); + while (!wifi_sta.is_connected()) { + std::this_thread::sleep_for(100ms); + } + logger.info("WiFi connected, local IP {}", ip_address); + + const std::string node_name = CONFIG_RTPS_EXAMPLE_NODE_NAME; + const std::string topic_prefix = CONFIG_RTPS_EXAMPLE_TOPIC_PREFIX; + const std::string request_topic = topic_prefix + "/request"; + const std::string response_topic = topic_prefix + "/response"; + + std::atomic request_count{0}; + std::atomic response_count{0}; + std::atomic next_request_value{1}; + std::atomic last_sent_request{0}; + espp::RtpsParticipant *participant_ptr = nullptr; + + espp::RtpsParticipant participant({ + .node_name = node_name, + .domain_id = CONFIG_RTPS_EXAMPLE_DOMAIN_ID, + .participant_id = CONFIG_RTPS_EXAMPLE_PARTICIPANT_ID, + .advertised_address = ip_address, + .announce_period = std::chrono::milliseconds(CONFIG_RTPS_EXAMPLE_ANNOUNCE_PERIOD_MS), + .on_participant_discovered = + [&logger](const auto &proxy) { + logger.info("Discovered participant '{}' at {} (meta {}, user {})", + proxy.name.empty() ? proxy.guid_prefix.to_string() : proxy.name, + proxy.address, proxy.ports.metatraffic_unicast, proxy.ports.user_unicast); + }, + .on_endpoint_discovered = + [&logger](const auto &endpoint) { + logger.info("Discovered remote {} '{}' [{}]", endpoint.is_reader ? "reader" : "writer", + endpoint.topic_name, endpoint.type_name); + }, + .log_level = espp::Logger::Verbosity::INFO, + }); + participant_ptr = &participant; + +#if CONFIG_RTPS_EXAMPLE_ROLE_INITIATOR + participant.add_writer({ + .topic_name = request_topic, + .type_name = std::string(kTypeName), + .reliability = espp::RtpsParticipant::ReliabilityKind::BEST_EFFORT, + .entity_index = 0, + }); + participant.add_reader({ + .topic_name = response_topic, + .type_name = std::string(kTypeName), + .reliability = espp::RtpsParticipant::ReliabilityKind::BEST_EFFORT, + .entity_index = 0, + .on_uint32_sample = + [&logger, &response_count, &last_sent_request](uint32_t value) { + response_count++; + logger.info("Received response {} (expected {})", value, last_sent_request.load()); + }, + }); +#else + participant.add_writer({ + .topic_name = response_topic, + .type_name = std::string(kTypeName), + .reliability = espp::RtpsParticipant::ReliabilityKind::BEST_EFFORT, + .entity_index = 0, + }); + participant.add_reader({ + .topic_name = request_topic, + .type_name = std::string(kTypeName), + .reliability = espp::RtpsParticipant::ReliabilityKind::BEST_EFFORT, + .entity_index = 0, + .on_uint32_sample = + [&logger, &request_count, &response_topic, &participant_ptr](uint32_t value) { + request_count++; + logger.info("Received request {}, sending response", value); + if (!participant_ptr->publish_uint32(response_topic, value)) { + logger.warn("Failed to publish response {}", value); + } + }, + }); +#endif + + auto ports = participant.ports(); + logger.info("Role: {}", +#if CONFIG_RTPS_EXAMPLE_ROLE_INITIATOR + "initiator" +#else + "responder" +#endif + ); + logger.info("Participant name: {}", node_name); + logger.info("Participant GUID: {}", participant.participant_guid().to_string()); + logger.info("Domain ID: {}, Participant ID: {}", CONFIG_RTPS_EXAMPLE_DOMAIN_ID, + CONFIG_RTPS_EXAMPLE_PARTICIPANT_ID); + logger.info("Topic prefix: {}", topic_prefix); + logger.info("Request topic: {}, Response topic: {}", request_topic, response_topic); + logger.info("Ports: meta mc={}, meta uc={}, user mc={}, user uc={}", ports.metatraffic_multicast, + ports.metatraffic_unicast, ports.user_multicast, ports.user_unicast); + + if (!run_local_protocol_checks(logger, participant)) { + return; + } + + if (!participant.start()) { + logger.error("Failed to start RTPS participant"); + return; + } + +#if CONFIG_RTPS_EXAMPLE_ROLE_INITIATOR + logger.info("Initiator is waiting for a responder on the same domain/topic prefix..."); + while (true) { + auto remote_readers = participant.discovered_readers(); + auto remote_writers = participant.discovered_writers(); + bool request_reader_ready = has_endpoint(remote_readers, request_topic, true); + bool response_writer_ready = has_endpoint(remote_writers, response_topic, false); + if (!request_reader_ready || !response_writer_ready) { + logger.info("Waiting for responder endpoints (request_reader={}, response_writer={})", + request_reader_ready, response_writer_ready); + std::this_thread::sleep_for(2s); + continue; + } + + auto value = next_request_value.fetch_add(1); + last_sent_request = value; + if (participant.publish_uint32(request_topic, value)) { + logger.info("Published request {} on '{}'", value, request_topic); + } else { + logger.warn("Failed to publish request {}", value); + } + std::this_thread::sleep_for(std::chrono::milliseconds(CONFIG_RTPS_EXAMPLE_PUBLISH_PERIOD_MS)); + } +#else + logger.info("Responder is ready and will echo '{}' samples back on '{}'", request_topic, + response_topic); + while (true) { + std::this_thread::sleep_for(5s); + logger.info("Responder status: discovered participants={}, requests handled={}", + participant.discovered_participants().size(), request_count.load()); + } +#endif +} diff --git a/components/rtps/example/partitions.csv b/components/rtps/example/partitions.csv new file mode 100644 index 000000000..c4217ab9e --- /dev/null +++ b/components/rtps/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/example/sdkconfig.defaults b/components/rtps/example/sdkconfig.defaults new file mode 100644 index 000000000..e823df850 --- /dev/null +++ b/components/rtps/example/sdkconfig.defaults @@ -0,0 +1,13 @@ +# Common ESP-related +# +CONFIG_ESP_SYSTEM_EVENT_TASK_STACK_SIZE=4096 +CONFIG_ESP_MAIN_TASK_STACK_SIZE=8192 + +# +# Partition Table +# +CONFIG_PARTITION_TABLE_CUSTOM=y +CONFIG_PARTITION_TABLE_CUSTOM_FILENAME="partitions.csv" +CONFIG_PARTITION_TABLE_FILENAME="partitions.csv" +CONFIG_PARTITION_TABLE_OFFSET=0x8000 +CONFIG_PARTITION_TABLE_MD5=y diff --git a/components/rtps/idf_component.yml b/components/rtps/idf_component.yml new file mode 100644 index 000000000..094c82efa --- /dev/null +++ b/components/rtps/idf_component.yml @@ -0,0 +1,26 @@ +## IDF Component Manager Manifest File +license: "MIT" +description: "Cross-platform RTPS protocol foundation component for ESP-IDF" +url: "https://github.com/esp-cpp/espp/tree/main/components/rtps" +repository: "git://github.com/esp-cpp/espp.git" +maintainers: + - William Emfinger +documentation: "https://esp-cpp.github.io/espp/rtps.html" +examples: + - path: example +tags: + - cpp + - Component + - RTPS + - DDS + - ROS2 + - UDP + - Discovery + - PubSub +dependencies: + idf: + version: '>=5.0' + espp/base_component: '>=1.0' + espp/cdr: '>=1.0' + espp/socket: '>=1.0' + espp/task: '>=1.0' diff --git a/components/rtps/include/rtps.hpp b/components/rtps/include/rtps.hpp new file mode 100644 index 000000000..6e8a308da --- /dev/null +++ b/components/rtps/include/rtps.hpp @@ -0,0 +1,370 @@ +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "base_component.hpp" +#include "task.hpp" +#include "udp_socket.hpp" + +namespace espp { +/// Cross-platform RTPS protocol foundation built on top of the socket component. +class RtpsParticipant : public BaseComponent { +public: + /// @brief Delivery semantics advertised for a writer or reader endpoint. + enum class ReliabilityKind : uint8_t { + BEST_EFFORT = 0, ///< Best-effort delivery semantics. + RELIABLE = 1, ///< Reliable delivery semantics. + }; + + /// @brief RTPS protocol version carried in the RTPS message header. + struct ProtocolVersion { + uint8_t major{2}; ///< Major RTPS version number. + uint8_t minor{3}; ///< Minor RTPS version number. + }; + + /// @brief RTPS vendor identifier carried in the RTPS message header. + struct VendorId { + std::array value{0xca, 0xfe}; ///< Two-byte vendor identifier. + }; + + /// @brief 12-byte prefix that identifies an RTPS participant. + struct GuidPrefix { + std::array value{}; ///< Raw 12-byte GUID prefix value. + + /// @brief Compare two GUID prefixes for equality. + /// @param other Prefix to compare against. + /// @return True if both prefixes contain identical bytes. + bool operator==(const GuidPrefix &other) const = default; + + /// @brief Convert the GUID prefix to a printable hex string. + /// @return Colon-separated hexadecimal representation of the prefix. + std::string to_string() const; + }; + + /// @brief 4-byte entity identifier within a participant. + struct EntityId { + std::array value{}; ///< Raw 4-byte entity identifier value. + + /// @brief Compare two entity identifiers for equality. + /// @param other Entity identifier to compare against. + /// @return True if both entity identifiers contain identical bytes. + bool operator==(const EntityId &other) const = default; + + /// @brief Convert the entity identifier to a printable hex string. + /// @return Colon-separated hexadecimal representation of the entity identifier. + std::string to_string() const; + }; + + /// @brief Globally unique identifier for an RTPS entity. + struct Guid { + GuidPrefix prefix{}; ///< Participant GUID prefix portion. + EntityId entity_id{}; ///< Entity identifier portion within the participant. + + /// @brief Compare two GUIDs for equality. + /// @param other GUID to compare against. + /// @return True if both GUIDs have the same prefix and entity identifier. + bool operator==(const Guid &other) const = default; + + /// @brief Convert the GUID to a printable string. + /// @return Combined printable representation of the prefix and entity identifier. + std::string to_string() const; + }; + + /// @brief RTPS sequence number wrapper. + struct SequenceNumber { + int64_t value{1}; ///< Signed 64-bit RTPS sequence number value. + }; + + /// @brief RTPS network locator for a unicast or multicast transport endpoint. + struct Locator { + /// @brief Supported locator transport kinds. + enum class Kind : int32_t { + INVALID = -1, ///< Locator is not initialized or not valid. + UDP_V4 = 1, ///< UDP over IPv4 locator. + }; + + Kind kind{Kind::INVALID}; ///< Transport kind of this locator. + uint32_t port{0}; ///< Transport port number in host byte order. + std::array address{}; ///< Raw 16-byte RTPS locator address field. + + /// @brief Build a UDPv4 locator from a dotted IPv4 address and port. + /// @param ipv4_address Dotted-decimal IPv4 address string. + /// @param port UDP port number to advertise. + /// @return A locator configured for UDPv4 with the IPv4 address stored in RTPS locator form. + static Locator udp_v4(std::string_view ipv4_address, uint16_t port); + + /// @brief Convert the locator address to a printable IPv4 string. + /// @return Dotted-decimal IPv4 address, or `0.0.0.0` if the locator is not UDPv4. + std::string address_string() const; + }; + + /// @brief RTPS message header fields. + struct Header { + ProtocolVersion protocol_version{}; ///< RTPS protocol version. + VendorId vendor_id{}; ///< Sender vendor identifier. + GuidPrefix guid_prefix{}; ///< Sender participant GUID prefix. + }; + + /// @brief Supported RTPS submessage kinds used by the current implementation. + enum class SubmessageKind : uint8_t { + PAD = 0x01, ///< Padding submessage. + ACKNACK = 0x06, ///< Reliable-reader acknowledgement submessage. + HEARTBEAT = 0x07, ///< Reliable-writer heartbeat submessage. + INFO_TS = 0x09, ///< Timestamp information submessage. + INFO_DST = 0x0e, ///< Destination GUID-prefix information submessage. + DATA = 0x15, ///< User or discovery data submessage. + }; + + /// @brief One RTPS submessage within an RTPS message. + struct Submessage { + SubmessageKind kind{SubmessageKind::PAD}; ///< Submessage kind discriminator. + uint8_t flags{0x01}; ///< Raw RTPS submessage flags byte. + std::vector + payload{}; ///< Serialized submessage payload bytes without the 4-byte submessage header. + }; + + /// @brief RTPS message consisting of a header and a sequence of submessages. + struct Message { + Header header{}; ///< RTPS message header. + std::vector submessages{}; ///< Serialized submessages carried by this RTPS message. + + /// @brief Serialize the RTPS message to bytes. + /// @return A complete RTPS message buffer ready to send on the network. + std::vector serialize() const; + + /// @brief Parse an RTPS message from bytes. + /// @param data Serialized RTPS message bytes. + /// @return A parsed message on success, or `std::nullopt` if the input is invalid. + static std::optional parse(std::span data); + }; + + /// @brief Standard RTPS UDP port mapping derived from domain and participant IDs. + struct PortMapping { + uint16_t metatraffic_multicast{0}; ///< Multicast discovery/metatraffic port. + uint16_t metatraffic_unicast{0}; ///< Unicast discovery/metatraffic port for this participant. + uint16_t user_multicast{0}; ///< Multicast user-data port. + uint16_t user_unicast{0}; ///< Unicast user-data port for this participant. + }; + + /// @brief Configuration for a locally advertised writer endpoint. + struct WriterConfig { + std::string topic_name{}; ///< Topic name advertised through SEDP. + std::string type_name{"std_msgs/msg/UInt32"}; ///< Type name advertised through SEDP. + ReliabilityKind reliability{ + ReliabilityKind::BEST_EFFORT}; ///< Reliability QoS advertised for the writer. + uint32_t entity_index{0}; ///< Local entity slot used to derive the RTPS entity ID. + }; + + /// @brief Configuration for a locally advertised reader endpoint. + struct ReaderConfig { + std::string topic_name{}; ///< Topic name advertised through SEDP. + std::string type_name{"std_msgs/msg/UInt32"}; ///< Type name advertised through SEDP. + ReliabilityKind reliability{ + ReliabilityKind::BEST_EFFORT}; ///< Reliability QoS advertised for the reader. + uint32_t entity_index{0}; ///< Local entity slot used to derive the RTPS entity ID. + std::function on_uint32_sample{ + nullptr}; ///< Callback invoked when a matching temporary UInt32 sample is received. + }; + + /// @brief Cached information about a discovered remote participant. + struct ParticipantProxy { + Guid participant_guid{}; ///< Discovered participant GUID. + GuidPrefix guid_prefix{}; ///< Discovered participant GUID prefix. + std::string name{}; ///< Remote participant name, if advertised. + std::string enclave{"/"}; ///< Remote ROS 2 enclave/user-data hint, if advertised. + std::string address{}; ///< Preferred remote IPv4 address for user traffic. + PortMapping ports{}; ///< Remote participant port mapping derived from discovery data. + uint32_t builtin_endpoints{0}; ///< Remote builtin-endpoint bitmask from SPDP. + }; + + /// @brief Cached information about a discovered remote reader or writer endpoint. + struct EndpointProxy { + Guid guid{}; ///< Discovered endpoint GUID. + Guid participant_guid{}; ///< GUID of the participant that owns this endpoint. + std::string topic_name{}; ///< Discovered topic name. + std::string type_name{"std_msgs/msg/UInt32"}; ///< Discovered type name. + ReliabilityKind reliability{ReliabilityKind::BEST_EFFORT}; ///< Advertised endpoint reliability. + bool is_reader{false}; ///< True for discovered readers, false for discovered writers. + bool expects_inline_qos{false}; ///< Whether the remote endpoint requested inline QoS. + Locator unicast_locator{}; ///< Preferred unicast locator advertised by the endpoint. + }; + + /// @brief Top-level participant configuration. + struct Config { + std::string node_name{"espp_rtps"}; ///< Local participant name advertised in discovery. + uint16_t domain_id{0}; ///< RTPS domain ID used for port derivation and discovery scope. + uint16_t participant_id{0}; ///< RTPS participant ID used for GUID and port derivation. + std::string bind_address{"0.0.0.0"}; ///< Local IPv4 address to bind sockets to. + std::string advertised_address{ + "127.0.0.1"}; ///< IPv4 address advertised to peers for unicast traffic. + std::string metatraffic_multicast_group{ + "239.255.0.1"}; ///< Multicast group used for RTPS metatraffic discovery. + Task::BaseConfig receive_task_config{ + .name = "RtpsRx", + .stack_size_bytes = 6 * 1024}; ///< Base task configuration for receive sockets. + Task::BaseConfig announce_task_config{ + .name = "RtpsAnnounce", + .stack_size_bytes = 6 * 1024}; ///< Task configuration for periodic discovery announcements. + std::chrono::milliseconds announce_period{1000}; ///< Interval between periodic SPDP/SEDP sends. + std::string enclave{"/"}; ///< User-data enclave string advertised in SPDP. + std::function on_participant_discovered{ + nullptr}; ///< Callback invoked when a remote participant is first discovered. + std::function on_endpoint_discovered{ + nullptr}; ///< Callback invoked when a remote endpoint is first discovered. + espp::Logger::Verbosity log_level{ + espp::Logger::Verbosity::INFO}; ///< Participant log verbosity. + }; + + /// @brief Construct an RTPS participant. + /// @param config Participant configuration controlling ports, addresses, discovery, and + /// callbacks. + explicit RtpsParticipant(const Config &config); + + /// @brief Destroy the participant and stop any active sockets/tasks. + ~RtpsParticipant(); + + /// @brief Start discovery sockets, user-data sockets, and periodic announcements. + /// @return True if startup succeeded, false if the participant was already started or + /// initialization failed. + bool start(); + + /// @brief Stop sockets and background tasks associated with the participant. + void stop(); + + /// @brief Check whether the participant is currently started. + /// @return True if the participant has been started and not yet stopped. + bool is_started() const; + + /// @brief Register a local writer endpoint to advertise through SEDP. + /// @param writer_config Writer configuration to add. + /// @return True after the writer has been stored. + bool add_writer(const WriterConfig &writer_config); + + /// @brief Register a local reader endpoint to advertise through SEDP. + /// @param reader_config Reader configuration to add. + /// @return True after the reader has been stored. + bool add_reader(const ReaderConfig &reader_config); + + /// @brief Get the currently discovered remote participants. + /// @return A snapshot copy of the discovered participant list. + std::vector discovered_participants() const; + + /// @brief Get the currently discovered remote writer endpoints. + /// @return A snapshot copy of the discovered writer list. + std::vector discovered_writers() const; + + /// @brief Get the currently discovered remote reader endpoints. + /// @return A snapshot copy of the discovered reader list. + std::vector discovered_readers() const; + + /// @brief Access the registered local writer configurations. + /// @return A const reference to the local writer list. + const std::vector &writers() const; + + /// @brief Access the registered local reader configurations. + /// @return A const reference to the local reader list. + const std::vector &readers() const; + + /// @brief Compute the standard RTPS UDP port mapping for this participant. + /// @return The derived metatraffic and user-data ports for the configured domain and participant + /// IDs. + PortMapping ports() const; + + /// @brief Get the local participant GUID. + /// @return GUID built from the local GUID prefix and participant entity ID. + Guid participant_guid() const; + + /// @brief Get the GUID for a local writer entity slot. + /// @param index Zero-based local writer entity index. + /// @return GUID for the derived local writer entity. + Guid writer_guid(size_t index) const; + + /// @brief Get the GUID for a local reader entity slot. + /// @param index Zero-based local reader entity index. + /// @return GUID for the derived local reader entity. + Guid reader_guid(size_t index) const; + + /// @brief Build the default participant announce message. + /// @return Serialized SPDP participant announce message. + std::vector build_announce_message() const; + + /// @brief Build the SPDP participant announcement message for this participant. + /// @return Serialized SPDP message describing the local participant. + std::vector build_spdp_announce_message() const; + + /// @brief Build an SEDP publication announcement for a local writer. + /// @param writer_config Writer configuration to serialize. + /// @return Serialized SEDP publication message for the writer. + std::vector build_sedp_publication_message(const WriterConfig &writer_config) const; + + /// @brief Build an SEDP subscription announcement for a local reader. + /// @param reader_config Reader configuration to serialize. + /// @return Serialized SEDP subscription message for the reader. + std::vector build_sedp_subscription_message(const ReaderConfig &reader_config) const; + + /// @brief Build a temporary ESPP UInt32 user-data message. + /// @param topic_name Topic name to embed in the message payload. + /// @param value UInt32 sample value to serialize. + /// @param reliability Reliability flag to encode in the temporary payload header. + /// @return Serialized RTPS DATA message containing the temporary ESPP UInt32 payload. + std::vector build_uint32_data_message(std::string_view topic_name, uint32_t value, + ReliabilityKind reliability) const; + + /// @brief Publish a temporary ESPP UInt32 sample to discovered participants. + /// @param topic_name Topic name to publish on. Must match a registered local writer. + /// @param value UInt32 sample value to send. + /// @return True if at least one send call succeeded, false otherwise. + bool publish_uint32(std::string_view topic_name, uint32_t value); + + /// @brief Serialize a UInt32 value into a standalone CDR payload. + /// @param value Value to serialize. + /// @return Encapsulated little-endian CDR payload containing the value. + static std::vector serialize_uint32_cdr(uint32_t value); + + /// @brief Parse a standalone CDR payload containing a UInt32 value. + /// @param data Encapsulated CDR payload bytes. + /// @return Parsed UInt32 value on success, or `std::nullopt` if the payload is invalid. + static std::optional deserialize_uint32_cdr(std::span data); + + /// @brief Compute the standard RTPS UDP port mapping for a domain/participant pair. + /// @param domain_id RTPS domain ID. + /// @param participant_id RTPS participant ID. + /// @return Derived RTPS metatraffic and user-data ports. + static PortMapping compute_port_mapping(uint16_t domain_id, uint16_t participant_id); + +private: + bool handle_metatraffic_message(std::vector &data, const Socket::Info &sender); + bool handle_user_message(std::vector &data, const Socket::Info &sender); + bool send_spdp_announce_now(); + bool send_sedp_announcements_to(const ParticipantProxy &participant); + bool send_discovery_now(); + ParticipantProxy make_local_participant_proxy() const; + + Config config_; + GuidPrefix guid_prefix_{}; + std::atomic_bool started_{false}; + + std::unique_ptr metatraffic_multicast_receiver_; + std::unique_ptr metatraffic_unicast_receiver_; + std::unique_ptr user_unicast_receiver_; + std::unique_ptr announce_task_; + + mutable std::mutex mutex_; + std::vector writers_; + std::vector readers_; + std::vector discovered_participants_; + std::vector discovered_writers_; + std::vector discovered_readers_; +}; +} // namespace espp diff --git a/components/rtps/src/rtps.cpp b/components/rtps/src/rtps.cpp new file mode 100644 index 000000000..df526118c --- /dev/null +++ b/components/rtps/src/rtps.cpp @@ -0,0 +1,1413 @@ +#include "rtps.hpp" + +#include +#include +#include +#include +#include +#include + +#include "cdr.hpp" + +namespace { +constexpr std::array kRtpsMagic{'R', 'T', 'P', 'S'}; +constexpr std::array kUserDataMagic{'E', 'S', 'P', 'P', 'D', 'A', 'T', 'A'}; +constexpr uint8_t kUserDataVersion = 1; + +constexpr uint16_t kPortBase = 7400; +constexpr uint16_t kDomainGain = 250; +constexpr uint16_t kParticipantGain = 2; +constexpr uint16_t kMetatrafficMulticastOffset = 0; +constexpr uint16_t kMetatrafficUnicastOffset = 10; +constexpr uint16_t kUserMulticastOffset = 1; +constexpr uint16_t kUserUnicastOffset = 11; + +constexpr uint8_t kSubmessageFlagLittleEndian = 0x01; +constexpr uint8_t kSubmessageFlagInlineQos = 0x02; +constexpr uint8_t kSubmessageFlagData = 0x04; +constexpr uint16_t kDataSubmessageOctetsToInlineQos = 16; + +constexpr uint32_t kBuiltinEndpointParticipantAnnouncer = 1u << 0; +constexpr uint32_t kBuiltinEndpointParticipantDetector = 1u << 1; +constexpr uint32_t kBuiltinEndpointPublicationAnnouncer = 1u << 2; +constexpr uint32_t kBuiltinEndpointPublicationDetector = 1u << 3; +constexpr uint32_t kBuiltinEndpointSubscriptionAnnouncer = 1u << 4; +constexpr uint32_t kBuiltinEndpointSubscriptionDetector = 1u << 5; +constexpr uint32_t kBuiltinEndpointParticipantMessageWriter = 1u << 10; +constexpr uint32_t kBuiltinEndpointParticipantMessageReader = 1u << 11; +constexpr uint32_t kBuiltinEndpointSet = + kBuiltinEndpointParticipantAnnouncer | kBuiltinEndpointParticipantDetector | + kBuiltinEndpointPublicationAnnouncer | kBuiltinEndpointPublicationDetector | + kBuiltinEndpointSubscriptionAnnouncer | kBuiltinEndpointSubscriptionDetector | + kBuiltinEndpointParticipantMessageWriter | kBuiltinEndpointParticipantMessageReader; + +constexpr std::array kEntityIdUnknown{{0x00, 0x00, 0x00, 0x00}}; +constexpr std::array kParticipantEntityId{{0x00, 0x00, 0x01, 0xc1}}; +constexpr std::array kSpdpWriterEntityId{{0x00, 0x01, 0x00, 0xc2}}; +constexpr std::array kSpdpReaderEntityId{{0x00, 0x01, 0x00, 0xc7}}; +constexpr std::array kSedpPublicationsWriterEntityId{{0x00, 0x00, 0x03, 0xc2}}; +constexpr std::array kSedpPublicationsReaderEntityId{{0x00, 0x00, 0x03, 0xc7}}; +constexpr std::array kSedpSubscriptionsWriterEntityId{{0x00, 0x00, 0x04, 0xc2}}; +constexpr std::array kSedpSubscriptionsReaderEntityId{{0x00, 0x00, 0x04, 0xc7}}; +constexpr uint8_t kUserWriterNoKeyKind = 0x03; +constexpr uint8_t kUserReaderNoKeyKind = 0x04; + +constexpr uint32_t kHistoryKeepLast = 0; +constexpr uint32_t kReliabilityBestEffort = 1; +constexpr uint32_t kReliabilityReliable = 2; +constexpr uint32_t kDurabilityVolatile = 0; +constexpr uint32_t kLivelinessAutomatic = 0; +constexpr int32_t kDefaultLeaseDurationSeconds = 20; +constexpr uint32_t kDefaultLeaseDurationNanoseconds = 0; +constexpr int32_t kDefaultMaxBlockingSeconds = 0; +constexpr uint32_t kDefaultMaxBlockingNanoseconds = 100000000; +constexpr uint32_t kUInt32SerializedSize = 8; + +enum class ParameterId : uint16_t { + PID_SENTINEL = 0x0001, + PID_PARTICIPANT_LEASE_DURATION = 0x0002, + PID_TOPIC_NAME = 0x0005, + PID_TYPE_NAME = 0x0007, + PID_DOMAIN_ID = 0x000f, + PID_PROTOCOL_VERSION = 0x0015, + PID_VENDORID = 0x0016, + PID_DURABILITY = 0x001d, + PID_RELIABILITY = 0x001a, + PID_LIVELINESS = 0x001b, + PID_USER_DATA = 0x002c, + PID_UNICAST_LOCATOR = 0x002f, + PID_DEFAULT_UNICAST_LOCATOR = 0x0031, + PID_METATRAFFIC_UNICAST_LOCATOR = 0x0032, + PID_METATRAFFIC_MULTICAST_LOCATOR = 0x0033, + PID_MULTICAST_LOCATOR = 0x0030, + PID_EXPECTS_INLINE_QOS = 0x0043, + PID_DEFAULT_MULTICAST_LOCATOR = 0x0048, + PID_PARTICIPANT_GUID = 0x0050, + PID_BUILTIN_ENDPOINT_SET = 0x0058, + PID_ENDPOINT_GUID = 0x005a, + PID_TYPE_MAX_SIZE_SERIALIZED = 0x0060, + PID_ENTITY_NAME = 0x0062, + PID_KEY_HASH = 0x0070, + PID_HISTORY = 0x0040, +}; + +class ByteWriter { +public: + void append_bytes(std::span bytes) { + data_.insert(data_.end(), bytes.begin(), bytes.end()); + } + + template void append_bytes(const std::array &bytes) { + data_.insert(data_.end(), bytes.begin(), bytes.end()); + } + + template void append_chars(const std::array &bytes) { + data_.insert(data_.end(), bytes.begin(), bytes.end()); + } + + void append_u8(uint8_t value) { data_.push_back(value); } + + void append_u16_le(uint16_t value) { + data_.push_back(static_cast(value & 0xff)); + data_.push_back(static_cast((value >> 8) & 0xff)); + } + + void append_u16_be(uint16_t value) { + data_.push_back(static_cast((value >> 8) & 0xff)); + data_.push_back(static_cast(value & 0xff)); + } + + void append_u32_le(uint32_t value) { + for (int i = 0; i < 4; i++) { + data_.push_back(static_cast((value >> (8 * i)) & 0xff)); + } + } + + void append_i32_le(int32_t value) { append_u32_le(static_cast(value)); } + + void append_u32_be(uint32_t value) { + for (int i = 3; i >= 0; i--) { + data_.push_back(static_cast((value >> (8 * i)) & 0xff)); + } + } + + void append_sequence_number_le(int64_t value) { + auto high = static_cast(value >> 32); + auto low = static_cast(value & 0xffffffffu); + append_i32_le(high); + append_u32_le(low); + } + + size_t size() const { return data_.size(); } + + void align(size_t alignment) { + while (data_.size() % alignment != 0) { + data_.push_back(0); + } + } + + std::vector take() { return std::move(data_); } + +private: + std::vector data_; +}; + +class ByteReader { +public: + explicit ByteReader(std::span data) + : data_(data) {} + + bool read_u8(uint8_t &value) { + if (remaining() < 1) { + return false; + } + value = data_[offset_++]; + return true; + } + + bool read_u16_le(uint16_t &value) { + if (remaining() < 2) { + return false; + } + value = static_cast(data_[offset_]) | + static_cast(static_cast(data_[offset_ + 1]) << 8); + offset_ += 2; + return true; + } + + bool read_u32_le(uint32_t &value) { + if (remaining() < 4) { + return false; + } + value = static_cast(data_[offset_]) | + (static_cast(data_[offset_ + 1]) << 8) | + (static_cast(data_[offset_ + 2]) << 16) | + (static_cast(data_[offset_ + 3]) << 24); + offset_ += 4; + return true; + } + + bool read_i32_le(int32_t &value) { + uint32_t unsigned_value = 0; + if (!read_u32_le(unsigned_value)) { + return false; + } + value = static_cast(unsigned_value); + return true; + } + + bool read_u32_be(uint32_t &value) { + if (remaining() < 4) { + return false; + } + value = (static_cast(data_[offset_]) << 24) | + (static_cast(data_[offset_ + 1]) << 16) | + (static_cast(data_[offset_ + 2]) << 8) | + static_cast(data_[offset_ + 3]); + offset_ += 4; + return true; + } + + bool read_sequence_number_le(int64_t &value) { + int32_t high = 0; + uint32_t low = 0; + if (!read_i32_le(high) || !read_u32_le(low)) { + return false; + } + value = (static_cast(high) << 32) | low; + return true; + } + + bool read_bytes(std::span destination) { + if (remaining() < destination.size()) { + return false; + } + std::memcpy(destination.data(), data_.data() + offset_, destination.size()); + offset_ += destination.size(); + return true; + } + + std::span read_span(size_t length) { + if (remaining() < length) { + return {}; + } + auto span = data_.subspan(offset_, length); + offset_ += length; + return span; + } + + size_t remaining() const { return data_.size() - offset_; } + +private: + std::span data_; + size_t offset_{0}; +}; + +struct ParameterView { + ParameterId id{ParameterId::PID_SENTINEL}; + std::span value{}; +}; + +struct DataSubmessageView { + espp::RtpsParticipant::EntityId reader_id{}; + espp::RtpsParticipant::EntityId writer_id{}; + int64_t writer_sn{0}; + std::span serialized_payload{}; + bool inline_qos_present{false}; + bool data_present{false}; +}; + +std::string hex_string(std::span bytes) { + std::ostringstream stream; + stream << std::hex << std::setfill('0'); + for (size_t i = 0; i < bytes.size(); i++) { + if (i != 0) { + stream << ':'; + } + stream << std::setw(2) << static_cast(bytes[i]); + } + return stream.str(); +} + +bool parse_ipv4(std::string_view address, std::array &octets) { + std::array parsed{}; + size_t part_index = 0; + size_t cursor = 0; + while (cursor < address.size() && part_index < parsed.size()) { + auto next = address.find('.', cursor); + if (next == std::string_view::npos) { + next = address.size(); + } + if (next == cursor) { + return false; + } + unsigned value = 0; + for (size_t i = cursor; i < next; i++) { + if (!std::isdigit(static_cast(address[i]))) { + return false; + } + value = value * 10 + static_cast(address[i] - '0'); + if (value > 255) { + return false; + } + } + parsed[part_index++] = static_cast(value); + cursor = next + 1; + } + if (part_index != parsed.size() || cursor < address.size()) { + return false; + } + octets = parsed; + return true; +} + +void append_string(ByteWriter &writer, std::string_view text) { + writer.append_u16_le(static_cast(text.size())); + writer.append_bytes( + std::span{reinterpret_cast(text.data()), text.size()}); +} + +std::optional read_string(ByteReader &reader) { + uint16_t length = 0; + if (!reader.read_u16_le(length)) { + return std::nullopt; + } + auto span = reader.read_span(length); + if (span.size() != length) { + return std::nullopt; + } + return std::string(reinterpret_cast(span.data()), span.size()); +} + +void append_parameter_header(ByteWriter &writer, ParameterId id, uint16_t length) { + writer.append_u16_le(static_cast(id)); + writer.append_u16_le(length); +} + +void append_parameter_guid(ByteWriter &writer, ParameterId id, + const espp::RtpsParticipant::Guid &guid) { + append_parameter_header(writer, id, 16); + writer.append_bytes(guid.prefix.value); + writer.append_bytes(guid.entity_id.value); +} + +void append_parameter_protocol_version(ByteWriter &writer, + const espp::RtpsParticipant::ProtocolVersion &version) { + append_parameter_header(writer, ParameterId::PID_PROTOCOL_VERSION, 4); + writer.append_u8(version.major); + writer.append_u8(version.minor); + writer.append_u8(0); + writer.append_u8(0); +} + +void append_parameter_vendor_id(ByteWriter &writer, + const espp::RtpsParticipant::VendorId &vendor_id) { + append_parameter_header(writer, ParameterId::PID_VENDORID, 4); + writer.append_bytes(vendor_id.value); + writer.append_u8(0); + writer.append_u8(0); +} + +void append_parameter_u32(ByteWriter &writer, ParameterId id, uint32_t value) { + append_parameter_header(writer, id, 4); + writer.append_u32_le(value); +} + +void append_parameter_bool(ByteWriter &writer, ParameterId id, bool value) { + append_parameter_header(writer, id, 4); + writer.append_u8(value ? 1 : 0); + writer.append_u8(0); + writer.append_u8(0); + writer.append_u8(0); +} + +void append_parameter_duration(ByteWriter &writer, ParameterId id, int32_t seconds, + uint32_t nanoseconds) { + append_parameter_header(writer, id, 8); + writer.append_i32_le(seconds); + writer.append_u32_le(nanoseconds); +} + +void append_parameter_locator(ByteWriter &writer, ParameterId id, + const espp::RtpsParticipant::Locator &locator) { + append_parameter_header(writer, id, 24); + writer.append_u32_be(static_cast(locator.kind)); + writer.append_u32_be(locator.port); + writer.append_bytes(locator.address); +} + +void append_parameter_string_cdr(ByteWriter &writer, ParameterId id, std::string_view text) { + uint16_t raw_length = static_cast(4 + text.size() + 1); + append_parameter_header(writer, id, raw_length); + auto cdr_writer = espp::CdrWriter::make_body_writer(espp::CdrEncapsulation::CDR_LE); + cdr_writer.write_string(text); + writer.append_bytes(cdr_writer.payload()); +} + +void append_parameter_octet_sequence(ByteWriter &writer, ParameterId id, + std::span bytes) { + uint16_t raw_length = static_cast(4 + bytes.size()); + append_parameter_header(writer, id, raw_length); + auto cdr_writer = espp::CdrWriter::make_body_writer(espp::CdrEncapsulation::CDR_LE); + cdr_writer.write(static_cast(bytes.size())); + cdr_writer.write_bytes(bytes); + cdr_writer.align(4); + writer.append_bytes(cdr_writer.payload()); +} + +void append_parameter_reliability(ByteWriter &writer, + espp::RtpsParticipant::ReliabilityKind reliability) { + append_parameter_header(writer, ParameterId::PID_RELIABILITY, 12); + writer.append_u32_le(reliability == espp::RtpsParticipant::ReliabilityKind::RELIABLE + ? kReliabilityReliable + : kReliabilityBestEffort); + writer.append_i32_le(kDefaultMaxBlockingSeconds); + writer.append_u32_le(kDefaultMaxBlockingNanoseconds); +} + +void append_parameter_durability(ByteWriter &writer) { + append_parameter_header(writer, ParameterId::PID_DURABILITY, 4); + writer.append_u32_le(kDurabilityVolatile); +} + +void append_parameter_liveliness(ByteWriter &writer) { + append_parameter_header(writer, ParameterId::PID_LIVELINESS, 12); + writer.append_u32_le(kLivelinessAutomatic); + writer.append_i32_le(kDefaultLeaseDurationSeconds); + writer.append_u32_le(kDefaultLeaseDurationNanoseconds); +} + +void append_parameter_history(ByteWriter &writer) { + append_parameter_header(writer, ParameterId::PID_HISTORY, 8); + writer.append_u32_le(kHistoryKeepLast); + writer.append_u32_le(1); +} + +void append_parameter_key_hash(ByteWriter &writer, const espp::RtpsParticipant::Guid &guid) { + append_parameter_header(writer, ParameterId::PID_KEY_HASH, 16); + writer.append_bytes(guid.prefix.value); + writer.append_bytes(guid.entity_id.value); +} + +void append_parameter_sentinel(ByteWriter &writer) { + append_parameter_header(writer, ParameterId::PID_SENTINEL, 0); +} + +std::vector parse_parameter_list(std::span payload) { + std::vector parameters; + espp::CdrReader cdr_reader(payload); + if (!cdr_reader.valid() || cdr_reader.encapsulation() != espp::CdrEncapsulation::PL_CDR_LE) { + return parameters; + } + + ByteReader reader(cdr_reader.payload()); + while (reader.remaining() >= 4) { + uint16_t pid = 0; + uint16_t length = 0; + if (!reader.read_u16_le(pid) || !reader.read_u16_le(length)) { + return {}; + } + if (pid == static_cast(ParameterId::PID_SENTINEL)) { + break; + } + auto value = reader.read_span(length); + if (value.size() != length) { + return {}; + } + parameters.push_back({.id = static_cast(pid), .value = value}); + auto padding = (4 - (length % 4)) & 0x3; + if (padding > 0 && reader.read_span(padding).size() != padding) { + return {}; + } + } + return parameters; +} + +std::optional find_parameter(std::span parameters, + ParameterId id) { + auto iterator = std::find_if(parameters.begin(), parameters.end(), + [id](const auto ¶meter) { return parameter.id == id; }); + if (iterator == parameters.end()) { + return std::nullopt; + } + return *iterator; +} + +std::optional parse_guid(std::span value) { + if (value.size() != 16) { + return std::nullopt; + } + espp::RtpsParticipant::Guid guid; + std::memcpy(guid.prefix.value.data(), value.data(), guid.prefix.value.size()); + std::memcpy(guid.entity_id.value.data(), value.data() + guid.prefix.value.size(), + guid.entity_id.value.size()); + return guid; +} + +std::optional parse_u32_le(std::span value) { + ByteReader reader(value); + uint32_t parsed = 0; + if (!reader.read_u32_le(parsed)) { + return std::nullopt; + } + return parsed; +} + +std::optional parse_bool(std::span value) { + if (value.size() < 1) { + return std::nullopt; + } + return value[0] != 0; +} + +std::optional parse_cdr_string(std::span value) { + auto reader = espp::CdrReader::make_body_reader(value, espp::CdrEncapsulation::CDR_LE); + if (!reader.valid()) { + return std::nullopt; + } + std::string text; + if (!reader.read_string(text)) { + return std::nullopt; + } + return text; +} + +std::optional> parse_octet_sequence(std::span value) { + auto reader = espp::CdrReader::make_body_reader(value, espp::CdrEncapsulation::CDR_LE); + if (!reader.valid()) { + return std::nullopt; + } + uint32_t length = 0; + if (!reader.read(length)) { + return std::nullopt; + } + std::vector bytes; + if (!reader.read_bytes(bytes, length)) { + return std::nullopt; + } + return bytes; +} + +std::optional parse_locator(std::span value) { + if (value.size() != 24) { + return std::nullopt; + } + ByteReader reader(value); + uint32_t kind = 0; + uint32_t port = 0; + espp::RtpsParticipant::Locator locator; + if (!reader.read_u32_be(kind) || !reader.read_u32_be(port) || + !reader.read_bytes(std::span{locator.address.data(), locator.address.size()})) { + return std::nullopt; + } + locator.kind = static_cast(static_cast(kind)); + locator.port = port; + return locator; +} + +std::optional +parse_reliability(std::span value) { + auto maybe_kind = parse_u32_le(value); + if (!maybe_kind) { + return std::nullopt; + } + if (*maybe_kind == kReliabilityReliable) { + return espp::RtpsParticipant::ReliabilityKind::RELIABLE; + } + return espp::RtpsParticipant::ReliabilityKind::BEST_EFFORT; +} + +std::string extract_enclave(std::span user_data_bytes) { + std::string text(reinterpret_cast(user_data_bytes.data()), user_data_bytes.size()); + std::string key = "enclave="; + auto position = text.find(key); + if (position == std::string::npos) { + return "/"; + } + position += key.size(); + auto end = text.find(';', position); + if (end == std::string::npos) { + end = text.size(); + } + return text.substr(position, end - position); +} + +std::array entity_id_for_index(uint32_t entity_index, uint8_t kind) { + return {0x00, 0x00, static_cast(0x10 + entity_index), kind}; +} + +bool is_same_guid_prefix(const espp::RtpsParticipant::Guid &guid, + const espp::RtpsParticipant::GuidPrefix &prefix) { + return guid.prefix == prefix; +} + +DataSubmessageView parse_data_submessage(const espp::RtpsParticipant::Submessage &submessage, + bool &ok) { + DataSubmessageView view; + ok = false; + if (submessage.kind != espp::RtpsParticipant::SubmessageKind::DATA || + (submessage.flags & kSubmessageFlagData) == 0) { + return view; + } + + ByteReader reader(std::span{submessage.payload.data(), submessage.payload.size()}); + uint16_t extra_flags = 0; + uint16_t octets_to_inline_qos = 0; + if (!reader.read_u16_le(extra_flags) || !reader.read_u16_le(octets_to_inline_qos) || + !reader.read_bytes( + std::span{view.reader_id.value.data(), view.reader_id.value.size()}) || + !reader.read_bytes( + std::span{view.writer_id.value.data(), view.writer_id.value.size()}) || + !reader.read_sequence_number_le(view.writer_sn)) { + return view; + } + + view.inline_qos_present = (submessage.flags & kSubmessageFlagInlineQos) != 0; + view.data_present = true; + if (view.inline_qos_present || octets_to_inline_qos != kDataSubmessageOctetsToInlineQos) { + return view; + } + + view.serialized_payload = reader.read_span(reader.remaining()); + ok = true; + return view; +} + +std::vector build_parameter_list_payload(ByteWriter ¶meter_writer) { + auto parameter_bytes = parameter_writer.take(); + return espp::CdrWriter::encapsulate(parameter_bytes, espp::CdrEncapsulation::PL_CDR_LE); +} + +std::vector build_data_submessage_payload(const espp::RtpsParticipant::EntityId &reader_id, + const espp::RtpsParticipant::EntityId &writer_id, + int64_t sequence_number, + std::span serialized_payload) { + ByteWriter writer; + writer.append_u16_le(0); + writer.append_u16_le(kDataSubmessageOctetsToInlineQos); + writer.append_bytes(reader_id.value); + writer.append_bytes(writer_id.value); + writer.append_sequence_number_le(sequence_number); + writer.append_bytes(serialized_payload); + writer.align(4); + return writer.take(); +} + +espp::RtpsParticipant::Message build_message(const espp::RtpsParticipant::GuidPrefix &guid_prefix, + const espp::RtpsParticipant::EntityId &reader_id, + const espp::RtpsParticipant::EntityId &writer_id, + int64_t sequence_number, + std::span serialized_payload) { + return {.header = {.guid_prefix = guid_prefix}, + .submessages = {{ + .kind = espp::RtpsParticipant::SubmessageKind::DATA, + .flags = static_cast(kSubmessageFlagLittleEndian | kSubmessageFlagData), + .payload = build_data_submessage_payload(reader_id, writer_id, sequence_number, + serialized_payload), + }}}; +} +} // namespace + +namespace espp { +std::string RtpsParticipant::GuidPrefix::to_string() const { return hex_string(value); } + +std::string RtpsParticipant::EntityId::to_string() const { return hex_string(value); } + +std::string RtpsParticipant::Guid::to_string() const { + return prefix.to_string() + '|' + entity_id.to_string(); +} + +RtpsParticipant::Locator RtpsParticipant::Locator::udp_v4(std::string_view ipv4_address, + uint16_t port) { + Locator locator; + locator.kind = Kind::UDP_V4; + locator.port = port; + std::array octets{}; + if (parse_ipv4(ipv4_address, octets)) { + locator.address[12] = octets[0]; + locator.address[13] = octets[1]; + locator.address[14] = octets[2]; + locator.address[15] = octets[3]; + } + return locator; +} + +std::string RtpsParticipant::Locator::address_string() const { + if (kind != Kind::UDP_V4) { + return "0.0.0.0"; + } + std::ostringstream stream; + stream << static_cast(address[12]) << '.' << static_cast(address[13]) << '.' + << static_cast(address[14]) << '.' << static_cast(address[15]); + return stream.str(); +} + +std::vector RtpsParticipant::Message::serialize() const { + ByteWriter writer; + writer.append_chars(kRtpsMagic); + writer.append_u8(header.protocol_version.major); + writer.append_u8(header.protocol_version.minor); + writer.append_bytes(header.vendor_id.value); + writer.append_bytes(header.guid_prefix.value); + for (const auto &submessage : submessages) { + writer.append_u8(static_cast(submessage.kind)); + writer.append_u8(submessage.flags); + writer.append_u16_le(static_cast(submessage.payload.size())); + writer.append_bytes(submessage.payload); + } + return writer.take(); +} + +std::optional +RtpsParticipant::Message::parse(std::span data) { + if (data.size() < 20 || !std::equal(kRtpsMagic.begin(), kRtpsMagic.end(), data.begin())) { + return std::nullopt; + } + + ByteReader reader(data.subspan(4)); + Message message; + if (!reader.read_u8(message.header.protocol_version.major) || + !reader.read_u8(message.header.protocol_version.minor) || + !reader.read_bytes(std::span{message.header.vendor_id.value.data(), + message.header.vendor_id.value.size()}) || + !reader.read_bytes(std::span{message.header.guid_prefix.value.data(), + message.header.guid_prefix.value.size()})) { + return std::nullopt; + } + + while (reader.remaining() > 0) { + Submessage submessage; + uint8_t kind = 0; + uint16_t length = 0; + if (!reader.read_u8(kind) || !reader.read_u8(submessage.flags) || !reader.read_u16_le(length)) { + return std::nullopt; + } + auto payload = reader.read_span(length); + if (payload.size() != length) { + return std::nullopt; + } + submessage.kind = static_cast(kind); + submessage.payload.assign(payload.begin(), payload.end()); + message.submessages.push_back(std::move(submessage)); + } + return message; +} + +RtpsParticipant::RtpsParticipant(const Config &config) + : BaseComponent({.tag = "RtpsParticipant", .level = config.log_level}) + , config_(config) { + auto hash = std::hash{}(config_.node_name); + guid_prefix_.value[0] = config_.participant_id & 0xff; + guid_prefix_.value[1] = (config_.participant_id >> 8) & 0xff; + guid_prefix_.value[2] = config_.domain_id & 0xff; + guid_prefix_.value[3] = (config_.domain_id >> 8) & 0xff; + for (size_t i = 0; i < 8; i++) { + guid_prefix_.value[4 + i] = static_cast((hash >> (8 * i)) & 0xff); + } +} + +RtpsParticipant::~RtpsParticipant() { stop(); } + +bool RtpsParticipant::start() { + if (started_.exchange(true)) { + return false; + } + + auto port_mapping = ports(); + metatraffic_multicast_receiver_ = + std::make_unique(UdpSocket::Config{.log_level = get_log_level()}); + metatraffic_unicast_receiver_ = + std::make_unique(UdpSocket::Config{.log_level = get_log_level()}); + user_unicast_receiver_ = + std::make_unique(UdpSocket::Config{.log_level = get_log_level()}); + + auto multicast_task_config = config_.receive_task_config; + multicast_task_config.name = config_.receive_task_config.name + "_spdp_mc"; + auto multicast_receive_config = UdpSocket::ReceiveConfig{ + .port = port_mapping.metatraffic_multicast, + .buffer_size = 4096, + .is_multicast_endpoint = true, + .multicast_group = config_.metatraffic_multicast_group, + .on_receive_callback = [this](auto &data, + const auto &sender) -> std::optional> { + handle_metatraffic_message(data, sender); + return std::nullopt; + }, + }; + if (!metatraffic_multicast_receiver_->start_receiving(multicast_task_config, + multicast_receive_config)) { + logger_.error("Failed to start metatraffic multicast receiver"); + stop(); + return false; + } + + auto unicast_meta_task_config = config_.receive_task_config; + unicast_meta_task_config.name = config_.receive_task_config.name + "_meta_uc"; + auto unicast_meta_receive_config = UdpSocket::ReceiveConfig{ + .port = port_mapping.metatraffic_unicast, + .buffer_size = 4096, + .on_receive_callback = [this](auto &data, + const auto &sender) -> std::optional> { + handle_metatraffic_message(data, sender); + return std::nullopt; + }, + }; + if (!metatraffic_unicast_receiver_->start_receiving(unicast_meta_task_config, + unicast_meta_receive_config)) { + logger_.error("Failed to start metatraffic unicast receiver"); + stop(); + return false; + } + + auto user_task_config = config_.receive_task_config; + user_task_config.name = config_.receive_task_config.name + "_user_uc"; + auto user_receive_config = UdpSocket::ReceiveConfig{ + .port = port_mapping.user_unicast, + .buffer_size = 4096, + .on_receive_callback = [this](auto &data, + const auto &sender) -> std::optional> { + handle_user_message(data, sender); + return std::nullopt; + }, + }; + if (!user_unicast_receiver_->start_receiving(user_task_config, user_receive_config)) { + logger_.error("Failed to start user unicast receiver"); + stop(); + return false; + } + + announce_task_ = Task::make_unique({ + .callback = [this](std::mutex &mutex, std::condition_variable &cv, bool ¬ified) -> bool { + send_discovery_now(); + std::unique_lock lock(mutex); + auto stop_requested = + cv.wait_for(lock, config_.announce_period, [¬ified] { return notified; }); + notified = false; + return stop_requested; + }, + .task_config = config_.announce_task_config, + .log_level = get_log_level(), + }); + announce_task_->start(); + send_discovery_now(); + return true; +} + +void RtpsParticipant::stop() { + started_ = false; + if (announce_task_) { + announce_task_->stop(); + announce_task_.reset(); + } + if (metatraffic_multicast_receiver_) { + metatraffic_multicast_receiver_->stop_receiving(); + metatraffic_multicast_receiver_.reset(); + } + if (metatraffic_unicast_receiver_) { + metatraffic_unicast_receiver_->stop_receiving(); + metatraffic_unicast_receiver_.reset(); + } + if (user_unicast_receiver_) { + user_unicast_receiver_->stop_receiving(); + user_unicast_receiver_.reset(); + } +} + +bool RtpsParticipant::is_started() const { return started_.load(); } + +bool RtpsParticipant::add_writer(const WriterConfig &writer_config) { + std::lock_guard lock(mutex_); + writers_.push_back(writer_config); + return true; +} + +bool RtpsParticipant::add_reader(const ReaderConfig &reader_config) { + std::lock_guard lock(mutex_); + readers_.push_back(reader_config); + return true; +} + +std::vector RtpsParticipant::discovered_participants() const { + std::lock_guard lock(mutex_); + return discovered_participants_; +} + +std::vector RtpsParticipant::discovered_writers() const { + std::lock_guard lock(mutex_); + return discovered_writers_; +} + +std::vector RtpsParticipant::discovered_readers() const { + std::lock_guard lock(mutex_); + return discovered_readers_; +} + +const std::vector &RtpsParticipant::writers() const { + return writers_; +} + +const std::vector &RtpsParticipant::readers() const { + return readers_; +} + +RtpsParticipant::PortMapping RtpsParticipant::ports() const { + return compute_port_mapping(config_.domain_id, config_.participant_id); +} + +RtpsParticipant::Guid RtpsParticipant::participant_guid() const { + return {.prefix = guid_prefix_, .entity_id = {.value = kParticipantEntityId}}; +} + +RtpsParticipant::Guid RtpsParticipant::writer_guid(size_t index) const { + return {.prefix = guid_prefix_, + .entity_id = { + .value = entity_id_for_index(static_cast(index), kUserWriterNoKeyKind)}}; +} + +RtpsParticipant::Guid RtpsParticipant::reader_guid(size_t index) const { + return {.prefix = guid_prefix_, + .entity_id = { + .value = entity_id_for_index(static_cast(index), kUserReaderNoKeyKind)}}; +} + +std::vector RtpsParticipant::build_announce_message() const { + return build_spdp_announce_message(); +} + +std::vector RtpsParticipant::build_spdp_announce_message() const { + ByteWriter parameters; + append_parameter_protocol_version(parameters, ProtocolVersion{}); + append_parameter_vendor_id(parameters, VendorId{}); + append_parameter_u32(parameters, ParameterId::PID_DOMAIN_ID, config_.domain_id); + append_parameter_guid(parameters, ParameterId::PID_PARTICIPANT_GUID, participant_guid()); + append_parameter_locator( + parameters, ParameterId::PID_METATRAFFIC_MULTICAST_LOCATOR, + Locator::udp_v4(config_.metatraffic_multicast_group, ports().metatraffic_multicast)); + append_parameter_locator( + parameters, ParameterId::PID_METATRAFFIC_UNICAST_LOCATOR, + Locator::udp_v4(config_.advertised_address, ports().metatraffic_unicast)); + append_parameter_locator(parameters, ParameterId::PID_DEFAULT_UNICAST_LOCATOR, + Locator::udp_v4(config_.advertised_address, ports().user_unicast)); + append_parameter_locator( + parameters, ParameterId::PID_DEFAULT_MULTICAST_LOCATOR, + Locator::udp_v4(config_.metatraffic_multicast_group, ports().user_multicast)); + append_parameter_duration(parameters, ParameterId::PID_PARTICIPANT_LEASE_DURATION, + kDefaultLeaseDurationSeconds, kDefaultLeaseDurationNanoseconds); + append_parameter_u32(parameters, ParameterId::PID_BUILTIN_ENDPOINT_SET, kBuiltinEndpointSet); + std::string enclave_text = "enclave=" + config_.enclave + ";"; + append_parameter_octet_sequence( + parameters, ParameterId::PID_USER_DATA, + std::span{reinterpret_cast(enclave_text.data()), + enclave_text.size()}); + append_parameter_string_cdr(parameters, ParameterId::PID_ENTITY_NAME, config_.node_name); + append_parameter_sentinel(parameters); + + auto payload = build_parameter_list_payload(parameters); + return build_message(guid_prefix_, {.value = kEntityIdUnknown}, {.value = kSpdpWriterEntityId}, 1, + payload) + .serialize(); +} + +std::vector +RtpsParticipant::build_sedp_publication_message(const WriterConfig &writer_config) const { + ByteWriter parameters; + auto guid = writer_guid(writer_config.entity_index); + append_parameter_guid(parameters, ParameterId::PID_ENDPOINT_GUID, guid); + append_parameter_locator(parameters, ParameterId::PID_UNICAST_LOCATOR, + Locator::udp_v4(config_.advertised_address, ports().user_unicast)); + append_parameter_guid(parameters, ParameterId::PID_PARTICIPANT_GUID, participant_guid()); + append_parameter_string_cdr(parameters, ParameterId::PID_TOPIC_NAME, writer_config.topic_name); + append_parameter_string_cdr(parameters, ParameterId::PID_TYPE_NAME, writer_config.type_name); + append_parameter_key_hash(parameters, guid); + append_parameter_u32(parameters, ParameterId::PID_TYPE_MAX_SIZE_SERIALIZED, + kUInt32SerializedSize); + append_parameter_protocol_version(parameters, ProtocolVersion{}); + append_parameter_vendor_id(parameters, VendorId{}); + append_parameter_durability(parameters); + append_parameter_liveliness(parameters); + append_parameter_reliability(parameters, writer_config.reliability); + append_parameter_history(parameters); + append_parameter_sentinel(parameters); + + auto payload = build_parameter_list_payload(parameters); + return build_message(guid_prefix_, {.value = kSedpPublicationsReaderEntityId}, + {.value = kSedpPublicationsWriterEntityId}, + static_cast(writer_config.entity_index + 1), payload) + .serialize(); +} + +std::vector +RtpsParticipant::build_sedp_subscription_message(const ReaderConfig &reader_config) const { + ByteWriter parameters; + auto guid = reader_guid(reader_config.entity_index); + append_parameter_guid(parameters, ParameterId::PID_ENDPOINT_GUID, guid); + append_parameter_locator(parameters, ParameterId::PID_UNICAST_LOCATOR, + Locator::udp_v4(config_.advertised_address, ports().user_unicast)); + append_parameter_bool(parameters, ParameterId::PID_EXPECTS_INLINE_QOS, false); + append_parameter_guid(parameters, ParameterId::PID_PARTICIPANT_GUID, participant_guid()); + append_parameter_string_cdr(parameters, ParameterId::PID_TOPIC_NAME, reader_config.topic_name); + append_parameter_string_cdr(parameters, ParameterId::PID_TYPE_NAME, reader_config.type_name); + append_parameter_key_hash(parameters, guid); + append_parameter_protocol_version(parameters, ProtocolVersion{}); + append_parameter_vendor_id(parameters, VendorId{}); + append_parameter_durability(parameters); + append_parameter_liveliness(parameters); + append_parameter_reliability(parameters, reader_config.reliability); + append_parameter_history(parameters); + append_parameter_sentinel(parameters); + + auto payload = build_parameter_list_payload(parameters); + return build_message(guid_prefix_, {.value = kSedpSubscriptionsReaderEntityId}, + {.value = kSedpSubscriptionsWriterEntityId}, + static_cast(reader_config.entity_index + 1), payload) + .serialize(); +} + +std::vector RtpsParticipant::build_uint32_data_message(std::string_view topic_name, + uint32_t value, + ReliabilityKind reliability) const { + ByteWriter payload_writer; + payload_writer.append_chars(kUserDataMagic); + payload_writer.append_u8(kUserDataVersion); + payload_writer.append_u8(static_cast(reliability)); + append_string(payload_writer, topic_name); + auto cdr = serialize_uint32_cdr(value); + payload_writer.append_u16_le(static_cast(cdr.size())); + payload_writer.append_bytes(cdr); + + auto guid = writers_.empty() ? writer_guid(0) : writer_guid(writers_.front().entity_index); + return build_message(guid_prefix_, {.value = kEntityIdUnknown}, guid.entity_id, 1, + payload_writer.take()) + .serialize(); +} + +bool RtpsParticipant::publish_uint32(std::string_view topic_name, uint32_t value) { + WriterConfig writer_config; + { + std::lock_guard lock(mutex_); + auto iterator = + std::find_if(writers_.begin(), writers_.end(), + [topic_name](const auto &writer) { return writer.topic_name == topic_name; }); + if (iterator == writers_.end()) { + logger_.warn("No writer registered for topic '{}'", topic_name); + return false; + } + writer_config = *iterator; + } + + if (writer_config.reliability == ReliabilityKind::RELIABLE) { + logger_.warn("Reliable user-data retransmission is not implemented yet; sending best-effort"); + } + + auto payload = build_uint32_data_message(topic_name, value, writer_config.reliability); + auto participants = discovered_participants(); + if (participants.empty()) { + logger_.warn("No discovered participants available for topic '{}'", topic_name); + return false; + } + + if (!user_unicast_receiver_) { + return false; + } + + bool sent = false; + for (const auto &participant : participants) { + auto send_config = UdpSocket::SendConfig{ + .ip_address = participant.address, + .port = participant.ports.user_unicast, + }; + sent = user_unicast_receiver_->send(payload, send_config) || sent; + } + return sent; +} + +std::vector RtpsParticipant::serialize_uint32_cdr(uint32_t value) { + espp::CdrWriter writer({ + .encapsulation = espp::CdrEncapsulation::CDR_LE, + .include_encapsulation = true, + }); + writer.write(value); + return writer.take_buffer(); +} + +std::optional RtpsParticipant::deserialize_uint32_cdr(std::span data) { + espp::CdrReader reader(data); + if (!reader.valid()) { + return std::nullopt; + } + uint32_t value = 0; + if (!reader.read(value)) { + return std::nullopt; + } + return value; +} + +RtpsParticipant::PortMapping RtpsParticipant::compute_port_mapping(uint16_t domain_id, + uint16_t participant_id) { + auto base = static_cast(kPortBase) + static_cast(kDomainGain) * domain_id; + auto participant_offset = static_cast(kParticipantGain) * participant_id; + return {.metatraffic_multicast = static_cast(base + kMetatrafficMulticastOffset), + .metatraffic_unicast = + static_cast(base + kMetatrafficUnicastOffset + participant_offset), + .user_multicast = static_cast(base + kUserMulticastOffset), + .user_unicast = static_cast(base + kUserUnicastOffset + participant_offset)}; +} + +bool RtpsParticipant::handle_metatraffic_message(std::vector &data, + const Socket::Info &sender) { + auto message = Message::parse(data); + if (!message) { + return false; + } + + for (const auto &submessage : message->submessages) { + bool valid_data = false; + auto data_view = parse_data_submessage(submessage, valid_data); + if (!valid_data) { + continue; + } + + auto parameters = parse_parameter_list(data_view.serialized_payload); + if (parameters.empty()) { + continue; + } + + if (data_view.writer_id.value == kSpdpWriterEntityId) { + auto maybe_participant_guid_parameter = + find_parameter(parameters, ParameterId::PID_PARTICIPANT_GUID); + if (!maybe_participant_guid_parameter) { + continue; + } + auto maybe_participant_guid = parse_guid(maybe_participant_guid_parameter->value); + if (!maybe_participant_guid || is_same_guid_prefix(*maybe_participant_guid, guid_prefix_)) { + continue; + } + + ParticipantProxy participant; + participant.participant_guid = *maybe_participant_guid; + participant.guid_prefix = maybe_participant_guid->prefix; + participant.address = sender.address; + + if (auto maybe_name_parameter = find_parameter(parameters, ParameterId::PID_ENTITY_NAME)) { + if (auto maybe_name = parse_cdr_string(maybe_name_parameter->value)) { + participant.name = *maybe_name; + } + } + if (auto maybe_user_data_parameter = find_parameter(parameters, ParameterId::PID_USER_DATA)) { + if (auto maybe_user_data = parse_octet_sequence(maybe_user_data_parameter->value)) { + participant.enclave = extract_enclave(*maybe_user_data); + } + } + if (auto maybe_builtin_endpoint_parameter = + find_parameter(parameters, ParameterId::PID_BUILTIN_ENDPOINT_SET)) { + if (auto maybe_builtin_endpoints = parse_u32_le(maybe_builtin_endpoint_parameter->value)) { + participant.builtin_endpoints = *maybe_builtin_endpoints; + } + } + if (auto maybe_meta_unicast_parameter = + find_parameter(parameters, ParameterId::PID_METATRAFFIC_UNICAST_LOCATOR)) { + if (auto maybe_locator = parse_locator(maybe_meta_unicast_parameter->value)) { + participant.ports.metatraffic_unicast = static_cast(maybe_locator->port); + } + } + if (auto maybe_meta_multicast_parameter = + find_parameter(parameters, ParameterId::PID_METATRAFFIC_MULTICAST_LOCATOR)) { + if (auto maybe_locator = parse_locator(maybe_meta_multicast_parameter->value)) { + participant.ports.metatraffic_multicast = static_cast(maybe_locator->port); + } + } + if (auto maybe_default_unicast_parameter = + find_parameter(parameters, ParameterId::PID_DEFAULT_UNICAST_LOCATOR)) { + if (auto maybe_locator = parse_locator(maybe_default_unicast_parameter->value)) { + participant.ports.user_unicast = static_cast(maybe_locator->port); + participant.address = maybe_locator->address_string(); + } + } + if (auto maybe_default_multicast_parameter = + find_parameter(parameters, ParameterId::PID_DEFAULT_MULTICAST_LOCATOR)) { + if (auto maybe_locator = parse_locator(maybe_default_multicast_parameter->value)) { + participant.ports.user_multicast = static_cast(maybe_locator->port); + } + } + + std::function callback; + bool is_new_participant = false; + { + std::lock_guard lock(mutex_); + auto iterator = + std::find_if(discovered_participants_.begin(), discovered_participants_.end(), + [&participant](const auto &candidate) { + return candidate.participant_guid == participant.participant_guid; + }); + if (iterator == discovered_participants_.end()) { + discovered_participants_.push_back(participant); + is_new_participant = true; + } else { + *iterator = participant; + } + callback = config_.on_participant_discovered; + } + + logger_.info("SPDP discovered participant '{}' at {} (meta {}, user {})", + participant.name.empty() ? participant.guid_prefix.to_string() + : participant.name, + participant.address, participant.ports.metatraffic_unicast, + participant.ports.user_unicast); + if (is_new_participant) { + send_sedp_announcements_to(participant); + if (callback) { + callback(participant); + } + } + continue; + } + + bool is_reader = false; + if (data_view.writer_id.value == kSedpPublicationsWriterEntityId) { + is_reader = false; + } else if (data_view.writer_id.value == kSedpSubscriptionsWriterEntityId) { + is_reader = true; + } else { + continue; + } + + auto maybe_endpoint_guid_parameter = find_parameter(parameters, ParameterId::PID_ENDPOINT_GUID); + if (!maybe_endpoint_guid_parameter) { + continue; + } + auto maybe_endpoint_guid = parse_guid(maybe_endpoint_guid_parameter->value); + if (!maybe_endpoint_guid || is_same_guid_prefix(*maybe_endpoint_guid, guid_prefix_)) { + continue; + } + + EndpointProxy endpoint; + endpoint.guid = *maybe_endpoint_guid; + endpoint.is_reader = is_reader; + + if (auto maybe_participant_guid_parameter = + find_parameter(parameters, ParameterId::PID_PARTICIPANT_GUID)) { + if (auto maybe_participant_guid = parse_guid(maybe_participant_guid_parameter->value)) { + endpoint.participant_guid = *maybe_participant_guid; + } + } + if (endpoint.participant_guid.entity_id.value == std::array{}) { + endpoint.participant_guid = {.prefix = endpoint.guid.prefix, + .entity_id = {.value = kParticipantEntityId}}; + } + if (auto maybe_topic_name_parameter = find_parameter(parameters, ParameterId::PID_TOPIC_NAME)) { + if (auto maybe_topic_name = parse_cdr_string(maybe_topic_name_parameter->value)) { + endpoint.topic_name = *maybe_topic_name; + } + } + if (auto maybe_type_name_parameter = find_parameter(parameters, ParameterId::PID_TYPE_NAME)) { + if (auto maybe_type_name = parse_cdr_string(maybe_type_name_parameter->value)) { + endpoint.type_name = *maybe_type_name; + } + } + if (auto maybe_locator_parameter = + find_parameter(parameters, ParameterId::PID_UNICAST_LOCATOR)) { + if (auto maybe_locator = parse_locator(maybe_locator_parameter->value)) { + endpoint.unicast_locator = *maybe_locator; + } + } + if (auto maybe_reliability_parameter = + find_parameter(parameters, ParameterId::PID_RELIABILITY)) { + if (auto maybe_reliability = parse_reliability(maybe_reliability_parameter->value)) { + endpoint.reliability = *maybe_reliability; + } + } + if (auto maybe_inline_qos_parameter = + find_parameter(parameters, ParameterId::PID_EXPECTS_INLINE_QOS)) { + if (auto maybe_inline_qos = parse_bool(maybe_inline_qos_parameter->value)) { + endpoint.expects_inline_qos = *maybe_inline_qos; + } + } + + std::function endpoint_callback; + bool is_new_endpoint = false; + { + std::lock_guard lock(mutex_); + auto &endpoint_list = is_reader ? discovered_readers_ : discovered_writers_; + auto iterator = std::find_if( + endpoint_list.begin(), endpoint_list.end(), + [&endpoint](const auto &candidate) { return candidate.guid == endpoint.guid; }); + if (iterator == endpoint_list.end()) { + endpoint_list.push_back(endpoint); + is_new_endpoint = true; + } else { + *iterator = endpoint; + } + endpoint_callback = config_.on_endpoint_discovered; + } + + logger_.info("SEDP discovered {} '{}' [{}] from participant {}", + endpoint.is_reader ? "reader" : "writer", endpoint.topic_name, endpoint.type_name, + endpoint.participant_guid.to_string()); + if (is_new_endpoint && endpoint_callback) { + endpoint_callback(endpoint); + } + } + return false; +} + +bool RtpsParticipant::handle_user_message(std::vector &data, const Socket::Info &sender) { + auto message = Message::parse(data); + if (!message) { + return false; + } + + for (const auto &submessage : message->submessages) { + if (submessage.kind != SubmessageKind::DATA || + submessage.payload.size() < kUserDataMagic.size() + 2 || + !std::equal(kUserDataMagic.begin(), kUserDataMagic.end(), submessage.payload.begin())) { + continue; + } + + ByteReader reader( + std::span{submessage.payload.data(), submessage.payload.size()}); + std::array magic{}; + uint8_t version = 0; + uint8_t reliability = 0; + if (!reader.read_bytes(std::span{magic.data(), magic.size()}) || + !reader.read_u8(version) || !reader.read_u8(reliability)) { + continue; + } + auto topic_name = read_string(reader); + uint16_t payload_length = 0; + if (version != kUserDataVersion || !topic_name || !reader.read_u16_le(payload_length)) { + continue; + } + auto payload = reader.read_span(payload_length); + auto maybe_value = deserialize_uint32_cdr(payload); + if (!maybe_value) { + continue; + } + + if (static_cast(reliability) == ReliabilityKind::RELIABLE) { + logger_.warn( + "Received reliable topic '{}' from {}, but ACKNACK/HEARTBEAT is not implemented yet", + *topic_name, sender); + } + + std::vector> callbacks; + { + std::lock_guard lock(mutex_); + for (const auto &reader_config : readers_) { + if (reader_config.topic_name == *topic_name && reader_config.on_uint32_sample) { + callbacks.push_back(reader_config.on_uint32_sample); + } + } + } + for (const auto &callback : callbacks) { + callback(*maybe_value); + } + } + return false; +} + +bool RtpsParticipant::send_spdp_announce_now() { + if (!metatraffic_unicast_receiver_) { + return false; + } + auto payload = build_spdp_announce_message(); + auto send_config = UdpSocket::SendConfig{ + .ip_address = config_.metatraffic_multicast_group, + .port = ports().metatraffic_multicast, + .is_multicast_endpoint = true, + }; + return metatraffic_unicast_receiver_->send(payload, send_config); +} + +bool RtpsParticipant::send_sedp_announcements_to(const ParticipantProxy &participant) { + if (!metatraffic_unicast_receiver_ || participant.ports.metatraffic_unicast == 0 || + participant.address.empty()) { + return false; + } + + bool sent = false; + std::vector local_writers; + std::vector local_readers; + { + std::lock_guard lock(mutex_); + local_writers = writers_; + local_readers = readers_; + } + + for (const auto &writer_config : local_writers) { + auto payload = build_sedp_publication_message(writer_config); + auto send_config = UdpSocket::SendConfig{ + .ip_address = participant.address, + .port = participant.ports.metatraffic_unicast, + }; + sent = metatraffic_unicast_receiver_->send(payload, send_config) || sent; + } + for (const auto &reader_config : local_readers) { + auto payload = build_sedp_subscription_message(reader_config); + auto send_config = UdpSocket::SendConfig{ + .ip_address = participant.address, + .port = participant.ports.metatraffic_unicast, + }; + sent = metatraffic_unicast_receiver_->send(payload, send_config) || sent; + } + return sent; +} + +bool RtpsParticipant::send_discovery_now() { + auto sent = send_spdp_announce_now(); + auto participants = discovered_participants(); + for (const auto &participant : participants) { + sent = send_sedp_announcements_to(participant) || sent; + } + return sent; +} + +RtpsParticipant::ParticipantProxy RtpsParticipant::make_local_participant_proxy() const { + return {.participant_guid = participant_guid(), + .guid_prefix = guid_prefix_, + .name = config_.node_name, + .enclave = config_.enclave, + .address = config_.advertised_address, + .ports = ports(), + .builtin_endpoints = kBuiltinEndpointSet}; +} +} // namespace espp diff --git a/doc/Doxyfile b/doc/Doxyfile index 29f2cd66c..e9e5ba911 100755 --- a/doc/Doxyfile +++ b/doc/Doxyfile @@ -90,6 +90,7 @@ EXAMPLE_PATH = \ $(PROJECT_PATH)/components/button/example/main/button_example.cpp \ $(PROJECT_PATH)/components/byte90/example/main/byte90_example.cpp \ $(PROJECT_PATH)/components/chsc6x/example/main/chsc6x_example.cpp \ + $(PROJECT_PATH)/components/cdr/example/main/cdr_example.cpp \ $(PROJECT_PATH)/components/cli/example/main/cli_example.cpp \ $(PROJECT_PATH)/components/cobs/example/main/cobs_example.cpp \ $(PROJECT_PATH)/components/color/example/main/color_example.cpp \ @@ -145,6 +146,7 @@ EXAMPLE_PATH = \ $(PROJECT_PATH)/components/qwiicnes/example/main/qwiicnes_example.cpp \ $(PROJECT_PATH)/components/remote_debug/example/main/remote_debug_example.cpp \ $(PROJECT_PATH)/components/rmt/example/main/rmt_example.cpp \ + $(PROJECT_PATH)/components/rtps/example/main/rtps_example.cpp \ $(PROJECT_PATH)/components/rtsp/example/main/rtsp_example.cpp \ $(PROJECT_PATH)/components/ping/example/main/ping_example.cpp \ $(PROJECT_PATH)/components/runqueue/example/main/runqueue_example.cpp \ @@ -214,6 +216,7 @@ INPUT = \ $(PROJECT_PATH)/components/button/include/button.hpp \ $(PROJECT_PATH)/components/byte90/include/byte90.hpp \ $(PROJECT_PATH)/components/chsc6x/include/chsc6x.hpp \ + $(PROJECT_PATH)/components/cdr/include/cdr.hpp \ $(PROJECT_PATH)/components/cli/include/cli.hpp \ $(PROJECT_PATH)/components/cli/include/line_input.hpp \ $(PROJECT_PATH)/components/cobs/include/cobs.hpp \ @@ -325,6 +328,7 @@ INPUT = \ $(PROJECT_PATH)/components/remote_debug/include/remote_debug.hpp \ $(PROJECT_PATH)/components/rmt/include/rmt.hpp \ $(PROJECT_PATH)/components/rmt/include/rmt_encoder.hpp \ + $(PROJECT_PATH)/components/rtps/include/rtps.hpp \ $(PROJECT_PATH)/components/rtsp/include/generic_depacketizer.hpp \ $(PROJECT_PATH)/components/rtsp/include/generic_packetizer.hpp \ $(PROJECT_PATH)/components/rtsp/include/h264_depacketizer.hpp \ diff --git a/doc/conf_common.py b/doc/conf_common.py index e9b121279..6cc7137d2 100644 --- a/doc/conf_common.py +++ b/doc/conf_common.py @@ -5,8 +5,14 @@ 'esp_docs.esp_extensions.dummy_build_system', 'esp_docs.esp_extensions.run_doxygen', 'myst_parser', + 'sphinxcontrib.mermaid', ] +mermaid_output_format = 'raw' +mermaid_d3_zoom = True +mermaid_dark_theme = 'neutral' +mermaid_light_theme = 'neutral' + exclude_paterns = ['build', '_build', 'detail'] # link roles config diff --git a/doc/en/cdr.rst b/doc/en/cdr.rst new file mode 100644 index 000000000..6e2868dd2 --- /dev/null +++ b/doc/en/cdr.rst @@ -0,0 +1,32 @@ +CDR (Common Data Representation) +******************************** + +The ``cdr`` component provides a small, standalone Common Data Representation +reader/writer utility aimed at standards-oriented protocols such as DDS/RTPS. + +This initial slice focuses on the pieces needed to start building interoperable +payloads without forcing applications to adopt DDS or RTPS as a whole: + +- CDR / PL_CDR encapsulation identifiers +- endian-aware primitive serialization helpers +- CDR alignment and padding handling +- string helpers using the standard CDR length-prefix + null terminator layout +- body helpers for CDR fields embedded inside larger protocol elements +- fixed-array helpers and zero-copy payload/span views +- primitive sequence helpers +- standalone usage outside RTPS + +Current scope: + +- useful as a reusable building block for future DDS/RTPS payload work +- suitable for direct use in other protocols that want CDR-style payloads +- not yet a full XTypes / XCDR2 implementation + +.. toctree:: + + cdr_example + +API Reference +------------- + +.. include-build-file:: inc/cdr.inc diff --git a/doc/en/cdr_example.md b/doc/en/cdr_example.md new file mode 100644 index 000000000..84b095f9d --- /dev/null +++ b/doc/en/cdr_example.md @@ -0,0 +1,2 @@ +```{include} ../../components/cdr/example/README.md +``` diff --git a/doc/en/index.rst b/doc/en/index.rst index ff9c77cca..c2472b496 100755 --- a/doc/en/index.rst +++ b/doc/en/index.rst @@ -20,6 +20,7 @@ This is the documentation for esp-idf c++ components, ESPP (`espp Participant["RtpsParticipant"] + Participant --> SPDP["SPDP participant DATA"] + Participant --> SEDP["SEDP publication/subscription DATA"] + Participant --> User["User DATA submessages"] + SPDP --> MetaMC["Metatraffic multicast"] + SEDP --> MetaUC["Metatraffic unicast"] + User --> UserUC["User unicast"] + MetaMC --> Peer["Remote participant"] + MetaUC --> Peer + UserUC --> Peer + +Discovery Flow +-------------- + +At a high level, discovery proceeds like this: + +.. mermaid:: + + sequenceDiagram + participant A as Local participant + participant MC as 239.255.0.1 + participant B as Remote participant + A->>MC: SPDP DATA(participant GUID, locators, enclave, builtin endpoints) + MC-->>B: multicast delivery + B->>MC: SPDP DATA(its participant metadata) + MC-->>A: multicast delivery + A->>B: SEDP publication DATA(topic/type/reliability) + A->>B: SEDP subscription DATA(topic/type/reliability) + B->>A: SEDP publication/subscription DATA + Note over A,B: Matching user-data traffic can then use the user-unicast ports + +When ``handle_metatraffic_message()`` receives SPDP, it parses: + +- the remote participant GUID +- participant name +- the ``enclave=...;`` entry carried in ``PID_USER_DATA`` +- built-in endpoint bitmasks +- metatraffic and user-data locators + +For SEDP it parses the endpoint GUID, topic name, type name, reliability, +inline-QoS expectation, and unicast locator, then updates the discovered reader +or writer cache. + +Ports and Channels +------------------ + +The component follows the standard UDPv4 RTPS port mapping formula: + +.. list-table:: + :header-rows: 1 + + * - Channel + - Formula + - Domain 0, participant 0 + * - Metatraffic multicast + - ``7400 + 250 * domain + 0`` + - ``7400`` + * - Metatraffic unicast + - ``7400 + 250 * domain + 10 + 2 * participant`` + - ``7410`` + * - User multicast + - ``7400 + 250 * domain + 1`` + - ``7401`` + * - User unicast + - ``7400 + 250 * domain + 11 + 2 * participant`` + - ``7411`` + +Current ESPP Scope +------------------ + +The current implementation is intentionally focused on the first +interoperability milestone: + +- RTPS message framing and parsing +- SPDP participant discovery +- SEDP publication and subscription discovery +- participant / endpoint caches and discovery callbacks +- simple CDR little-endian serialization helpers for ``std_msgs/msg/UInt32`` + +The following pieces are **not finished yet**: + +- reliable RTPS state machines such as ``HEARTBEAT`` and ``ACKNACK`` +- standards-based ROS 2 user-data writers/readers +- full QoS matching beyond the currently emitted discovery parameters + +Feature Status +-------------- + +.. list-table:: + :header-rows: 1 + + * - Feature + - Status + - Notes + * - RTPS header / DATA submessage serialize + parse + - **Implemented** + - Core message framing is present. + * - Standard UDPv4 RTPS port mapping + - **Implemented** + - Uses the DDSI-RTPS well-known port formula. + * - SPDP participant announce send/receive + - **Implemented** + - Multicast announce plus participant cache updates. + * - SEDP publication / subscription announce send/receive + - **Implemented** + - Local endpoints are announced and remote endpoints are cached. + * - Participant / endpoint discovery callbacks + - **Implemented** + - Exposed through ``on_participant_discovered`` and + ``on_endpoint_discovered``. + * - Temporary ``UInt32`` user-data path + - **Implemented** + - Uses the current ESPP-specific ``ESPPDATA`` payload, not a + standards-based DDS sample representation. + * - QoS fields emitted in discovery + - **Partial** + - Reliability, durability, liveliness, and history parameters are + advertised in SEDP. + * - QoS matching / policy enforcement + - **Not implemented** + - Remote QoS is parsed, but full writer/reader matching logic is still + missing. + * - Standards-based DDS user-data serialization + - **Not implemented** + - The current data path is a temporary ESPP scaffold for + ``std_msgs/msg/UInt32``. + * - Inline QoS handling + - **Not implemented** + - Discovery and user-data handling assume no inline QoS. + * - Reliable RTPS (``HEARTBEAT``, ``ACKNACK``, resend) + - **Not implemented** + - Reliable delivery is not interoperable yet. + * - Full ROS 2 topic interoperability + - **Not implemented** + - Discovery is the current milestone; ROS 2-compatible data + writers/readers are still pending. + +Relevant Specifications +----------------------- + +These are the primary standards and references for understanding the current +implementation and the remaining work: + +.. list-table:: + :header-rows: 1 + + * - Specification + - Why it matters here + * - `OMG DDSI-RTPS 2.3 `_ + - Primary wire-level reference for RTPS headers, DATA submessages, SPDP, + SEDP, locator encoding, GUIDs, and the UDP port mapping used by this + component. + * - `OMG DDS 1.4 `_ + - Defines the conceptual participant, reader, writer, topic, and QoS model + that RTPS discovery is advertising. + +Example +------- + +The :doc:`rtps_example` page demonstrates the current discovery scaffold by: + +- computing the RTPS ports for a participant +- building and parsing locally generated SPDP and SEDP messages +- round-tripping a ``UInt32`` value through the CDR helper functions +- registering best-effort and reliable topic endpoints in the participant API + +.. toctree:: + + rtps_example + +API Reference +------------- + +.. include-build-file:: inc/rtps.inc diff --git a/doc/en/rtps_example.md b/doc/en/rtps_example.md new file mode 100644 index 000000000..d213860af --- /dev/null +++ b/doc/en/rtps_example.md @@ -0,0 +1,2 @@ +```{include} ../../components/rtps/example/README.md +``` diff --git a/doc/en/rtsp.rst b/doc/en/rtsp.rst index 3de33b8d6..6e0cb43ec 100644 --- a/doc/en/rtsp.rst +++ b/doc/en/rtsp.rst @@ -7,6 +7,84 @@ an extensible packetizer/depacketizer architecture. The component handles RTP packet splitting and reassembly; encoding and decoding of media data is handled externally by the application. +How RTSP Works +-------------- + +The component uses a split control-plane / media-plane design: + +- **RTSP over TCP** handles session control such as ``OPTIONS``, ``DESCRIBE``, + ``SETUP``, ``PLAY``, ``PAUSE``, and ``TEARDOWN``. +- **SDP** returned from ``DESCRIBE`` tells the client what tracks exist, how + they are encoded, and which per-track control URLs must be used for + ``SETUP``. +- **RTP/UDP** carries encoded media packets after playback starts. +- **RTCP/UDP** sockets are created alongside RTP sockets, but the current ESPP + implementation keeps RTCP support lightweight and does not yet implement a + full control/feedback plane. + +.. mermaid:: + + sequenceDiagram + participant App as Application + participant Server as RtspServer / RtspSession + participant Client as RtspClient + App->>Server: add_track() / send_frame() + Client->>Server: OPTIONS + Server-->>Client: 200 OK + Client->>Server: DESCRIBE + Server-->>Client: SDP with session + track control paths + Client->>Server: SETUP(trackID=n, client_port=RTP-RTCP) + Server-->>Client: Session + Transport headers + Client->>Server: PLAY + Server-->>Client: 200 OK + Server-->>Client: RTP/UDP packets for each active track + Client-->>App: on_jpeg_frame() or on_frame(track_id, data) + Client->>Server: TEARDOWN + Server-->>Client: 200 OK + +In ESPP, the server generates one SDP description per session, with one +``m=...`` section and one ``a=control:.../trackID=N`` entry per registered +track. The client parses those lines during ``describe()`` and then issues +``SETUP`` once per discovered track before calling ``PLAY``. + +Packetization Pipeline +---------------------- + +The codec-specific logic is intentionally separated from the RTSP core: + +.. mermaid:: + + flowchart LR + Frame["Encoded frame bytes"] --> Packetizer["Codec packetizer"] + Packetizer --> Chunks["RTP payload chunks"] + Chunks --> Header["RtspServer adds RTP headers"] + Header --> Session["RtspSession sends UDP packets"] + Session --> ClientRtp["RtspClient RTP socket"] + ClientRtp --> Depacketizer["Codec depacketizer"] + Depacketizer --> Callback["Application callback"] + +``RtspServer::send_frame(track_id, data)`` asks the selected packetizer to split +the encoded frame into MTU-sized chunks, adds RTP headers with track-specific +SSRC and sequence numbers, and leaves the resulting packets queued for active +sessions to transmit. On the client side, ``RtspClient::handle_rtp_packet()`` +parses the RTP header, uses the payload type to find the matching depacketizer, +and emits a completed frame through either ``on_jpeg_frame`` or the generic +``on_frame(track_id, data)`` callback. + +Legacy MJPEG Compatibility +-------------------------- + +For backward compatibility, the component still preserves the older MJPEG-only +behavior: + +- ``RtspServer::send_frame(std::span)`` lazily creates a default + track 0 and uses the legacy RFC 2435-compatible MJPEG wire format. +- ``RtspClient`` automatically creates an ``MjpegDepacketizer`` when a JPEG + callback is registered and payload type 26 is discovered in SDP. + +This means older single-track MJPEG integrations can keep working while newer +multi-track applications use ``add_track()`` plus codec-specific packetizers. + RTSP Client ----------- @@ -69,6 +147,37 @@ are provided for: Custom packetizers can be created by subclassing ``RtpPacketizer`` or ``RtpDepacketizer``. +Relevant Specifications +----------------------- + +These are the main standards to keep beside the code when working on this +component: + +.. list-table:: + :header-rows: 1 + + * - Specification + - Why it matters here + * - `RFC 2326: Real Time Streaming Protocol (RTSP) `_ + - Primary control-plane reference for the RTSP/1.0 request and response + flow implemented by ``RtspClient``, ``RtspServer``, and ``RtspSession``. + * - `RFC 7826: RTSP 2.0 `_ + - Useful background for newer RTSP deployments; informative here because + the current component speaks RTSP/1.0 on the wire. + * - `RFC 3550: RTP / RTCP `_ + - Defines RTP headers, timestamps, sequence numbers, SSRC handling, and + the RTCP control protocol model used by the transport layer. + * - `RFC 4566: Session Description Protocol (SDP) `_ + - Describes the SDP ``m=``, ``a=control:``, and ``a=rtpmap:`` lines that + the server generates and the client parses during ``DESCRIBE``. + * - `RFC 3551: RTP A/V Profile `_ + - Defines common RTP payload-type and clock-rate conventions used alongside + dynamic payloads. + * - `RFC 2435: RTP Payload Format for JPEG `_ + - Reference for the MJPEG packetization and depacketization path. + * - `RFC 6184: RTP Payload Format for H.264 Video `_ + - Reference for the H.264 FU-A fragmentation and reassembly path. + Testing and Utilities --------------------- diff --git a/doc/requirements.txt b/doc/requirements.txt index 82faccbef..0d157e36c 100644 --- a/doc/requirements.txt +++ b/doc/requirements.txt @@ -4,3 +4,4 @@ cairosvg esp-docs myst-parser +sphinxcontrib-mermaid diff --git a/docker_build_docs.sh b/docker_build_docs.sh index 9d5896cac..df9b11550 100755 --- a/docker_build_docs.sh +++ b/docker_build_docs.sh @@ -3,6 +3,8 @@ export PYTHONPATH=$PYTHONPATH:/project/doc # ensure we can run git commands git config --global --add safe.directory /project +# install documentation extensions not guaranteed to be present in the base image +python -m pip install --user -r /project/doc/requirements.txt # build the docs build-docs -bs html -t esp32 -l en --project-path /project/ --source-dir /project/doc/ --doxyfile_dir /project/doc/ if ! sh /project/doc/build_latex_pdf.sh /project/_build/en/esp32/latex; then diff --git a/python/README.md b/python/README.md index d4dfa9912..429c5653f 100644 --- a/python/README.md +++ b/python/README.md @@ -47,6 +47,10 @@ This section gives a brief overview of what the scripts in this folder do. audio by default when running with a UI; use `--no-audio-playback` to disable it or `--play-audio --headless` to exercise playback without opening the video window. +- `rtps_host.py`: A pure-stdlib host-side RTPS harness for discovering an ESPP + `RtpsParticipant`, printing SPDP/SEDP metadata, and optionally publishing or + receiving the current temporary `UInt32` user-data payloads without needing + Python bindings. - `cobs_demo.py`: Demonstration of ESPP COBS functionality with native Python data types. Shows ESPP encoding/decoding, cross-library compatibility with the cobs-python library, and practical usage examples. Includes design differences explanation and validation @@ -105,7 +109,9 @@ python3 .py python3 task.py # or python3 udp_client.py -``` +# or discover / test RTPS from a host machine +python3 rtps_host.py --advertised-address +``` Note: the `udp_client.py` script requires a running instance of the `udp_server.py` script. To run the server, use the following command from @@ -114,3 +120,23 @@ another terminal: ```console python3 udp_server.py ``` + +For the default ESP RTPS example, the host harness now defaults to the +**responder** side of the request/response test, so it will subscribe to +``espp/rtps_example/request`` and echo values back on +``espp/rtps_example/response``: + +```console +python3 rtps_host.py --advertised-address 192.168.1.50 +``` + +To act as the initiator instead, swap the topics and enable periodic publishing: + +```console +python3 rtps_host.py --advertised-address 192.168.1.50 \ + --subscribe-topic espp/rtps_example/response \ + --publish-topic espp/rtps_example/request \ + --publish-value 42 \ + --publish-interval 1.0 \ + --no-echo-received +``` diff --git a/python/rtps_host.py b/python/rtps_host.py new file mode 100644 index 000000000..81eef251b --- /dev/null +++ b/python/rtps_host.py @@ -0,0 +1,1022 @@ +#!/usr/bin/env python3 +"""Simple host-side RTPS test harness for the ESPP RTPS component. + +This script speaks the current ESPP RTPS discovery wire format plus the +temporary ``UInt32`` user-data payload used by ``RtpsParticipant`` today. It is +useful for: + +1. discovering an embedded ESPP RTPS participant from a PC/host, +2. inspecting SPDP/SEDP announcements, and +3. sending or receiving ``std_msgs/msg/UInt32``-style test samples over the + temporary ESPP user-data path. + +It uses only the Python standard library, so it does not require Python +bindings or a rebuilt host ``lib/`` tree. +""" + +from __future__ import annotations + +import argparse +import hashlib +import ipaddress +import select +import socket +import struct +import sys +import time +from dataclasses import dataclass +from typing import Dict, Iterable, List, Optional + + +RTPS_MAGIC = b"RTPS" +PL_CDR_LE = b"\x00\x03\x00\x00" +USER_DATA_MAGIC = b"ESPPDATA" +USER_DATA_VERSION = 1 + +PORT_BASE = 7400 +DOMAIN_GAIN = 250 +PARTICIPANT_GAIN = 2 +METATRAFFIC_MULTICAST_OFFSET = 0 +METATRAFFIC_UNICAST_OFFSET = 10 +USER_MULTICAST_OFFSET = 1 +USER_UNICAST_OFFSET = 11 + +DATA_SUBMESSAGE_KIND = 0x15 +DATA_SUBMESSAGE_FLAGS = 0x01 | 0x04 +DATA_SUBMESSAGE_OCTETS_TO_INLINE_QOS = 16 + +RELIABILITY_BEST_EFFORT = 1 +RELIABILITY_RELIABLE = 2 + +KIND_UDP_V4 = 1 +VENDOR_ID = b"\xca\xfe" + +ENTITY_ID_UNKNOWN = b"\x00\x00\x00\x00" +PARTICIPANT_ENTITY_ID = b"\x00\x00\x01\xc1" +SPDP_WRITER_ENTITY_ID = b"\x00\x01\x00\xc2" +SPDP_READER_ENTITY_ID = b"\x00\x01\x00\xc7" +SEDP_PUBLICATIONS_WRITER_ENTITY_ID = b"\x00\x00\x03\xc2" +SEDP_PUBLICATIONS_READER_ENTITY_ID = b"\x00\x00\x03\xc7" +SEDP_SUBSCRIPTIONS_WRITER_ENTITY_ID = b"\x00\x00\x04\xc2" +SEDP_SUBSCRIPTIONS_READER_ENTITY_ID = b"\x00\x00\x04\xc7" +USER_WRITER_NO_KEY_KIND = 0x03 +USER_READER_NO_KEY_KIND = 0x04 + +BUILTIN_ENDPOINT_SET = ( + (1 << 0) + | (1 << 1) + | (1 << 2) + | (1 << 3) + | (1 << 4) + | (1 << 5) + | (1 << 10) + | (1 << 11) +) + +PID_SENTINEL = 0x0001 +PID_PARTICIPANT_LEASE_DURATION = 0x0002 +PID_TOPIC_NAME = 0x0005 +PID_TYPE_NAME = 0x0007 +PID_DOMAIN_ID = 0x000F +PID_PROTOCOL_VERSION = 0x0015 +PID_VENDORID = 0x0016 +PID_RELIABILITY = 0x001A +PID_LIVELINESS = 0x001B +PID_DURABILITY = 0x001D +PID_USER_DATA = 0x002C +PID_UNICAST_LOCATOR = 0x002F +PID_DEFAULT_UNICAST_LOCATOR = 0x0031 +PID_METATRAFFIC_UNICAST_LOCATOR = 0x0032 +PID_METATRAFFIC_MULTICAST_LOCATOR = 0x0033 +PID_HISTORY = 0x0040 +PID_EXPECTS_INLINE_QOS = 0x0043 +PID_DEFAULT_MULTICAST_LOCATOR = 0x0048 +PID_PARTICIPANT_GUID = 0x0050 +PID_BUILTIN_ENDPOINT_SET = 0x0058 +PID_ENDPOINT_GUID = 0x005A +PID_TYPE_MAX_SIZE_SERIALIZED = 0x0060 +PID_ENTITY_NAME = 0x0062 +PID_KEY_HASH = 0x0070 + +DEFAULT_LEASE_DURATION_SECONDS = 20 +DEFAULT_LEASE_DURATION_NANOSECONDS = 0 +DEFAULT_MAX_BLOCKING_SECONDS = 0 +DEFAULT_MAX_BLOCKING_NANOSECONDS = 100_000_000 +DEFAULT_TOPIC_PREFIX = "espp/rtps_example" +DEFAULT_REQUEST_TOPIC = f"{DEFAULT_TOPIC_PREFIX}/request" +DEFAULT_RESPONSE_TOPIC = f"{DEFAULT_TOPIC_PREFIX}/response" + + +@dataclass +class PortMapping: + metatraffic_multicast: int + metatraffic_unicast: int + user_multicast: int + user_unicast: int + + +@dataclass +class ParticipantProxy: + participant_guid: bytes + guid_prefix: bytes + name: str + enclave: str + address: str + ports: PortMapping + builtin_endpoints: int + + +@dataclass +class EndpointProxy: + guid: bytes + participant_guid: bytes + topic_name: str + type_name: str + reliability: str + is_reader: bool + expects_inline_qos: bool + unicast_address: str + unicast_port: int + + +@dataclass +class WriterConfig: + topic_name: str + type_name: str + reliable: bool + entity_index: int + + +@dataclass +class ReaderConfig: + topic_name: str + type_name: str + reliable: bool + entity_index: int + + +def log(message: str) -> None: + print(message, flush=True) + + +def hex_string(value: bytes) -> str: + return value.hex() + + +def guid_to_string(guid: bytes) -> str: + return hex_string(guid[:12]) + ":" + hex_string(guid[12:]) + + +def entity_id_for_index(entity_index: int, kind: int) -> bytes: + return bytes((0x00, 0x00, 0x10 + entity_index, kind)) + + +def reliability_to_name(reliable: bool) -> str: + return "reliable" if reliable else "best-effort" + + +def compute_port_mapping(domain_id: int, participant_id: int) -> PortMapping: + base = PORT_BASE + DOMAIN_GAIN * domain_id + participant_offset = PARTICIPANT_GAIN * participant_id + return PortMapping( + metatraffic_multicast=base + METATRAFFIC_MULTICAST_OFFSET, + metatraffic_unicast=base + METATRAFFIC_UNICAST_OFFSET + participant_offset, + user_multicast=base + USER_MULTICAST_OFFSET, + user_unicast=base + USER_UNICAST_OFFSET + participant_offset, + ) + + +def guess_local_ipv4() -> str: + probe = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) + try: + probe.connect(("8.8.8.8", 80)) + return probe.getsockname()[0] + except OSError: + return "127.0.0.1" + finally: + probe.close() + + +def make_guid_prefix(node_name: str, domain_id: int, participant_id: int) -> bytes: + digest = hashlib.sha256(node_name.encode("utf-8")).digest() + return bytes( + ( + participant_id & 0xFF, + (participant_id >> 8) & 0xFF, + domain_id & 0xFF, + (domain_id >> 8) & 0xFF, + ) + ) + digest[:8] + + +def make_guid(prefix: bytes, entity_id: bytes) -> bytes: + return prefix + entity_id + + +def align4(buffer: bytearray) -> None: + while len(buffer) % 4 != 0: + buffer.append(0) + + +def append_parameter_header(buffer: bytearray, pid: int, length: int) -> None: + buffer.extend(struct.pack(" None: + append_parameter_header(buffer, pid, 16) + buffer.extend(guid) + + +def append_parameter_protocol_version(buffer: bytearray) -> None: + append_parameter_header(buffer, PID_PROTOCOL_VERSION, 4) + buffer.extend((2, 3, 0, 0)) + + +def append_parameter_vendor_id(buffer: bytearray) -> None: + append_parameter_header(buffer, PID_VENDORID, 4) + buffer.extend(VENDOR_ID) + buffer.extend((0, 0)) + + +def append_parameter_u32(buffer: bytearray, pid: int, value: int) -> None: + append_parameter_header(buffer, pid, 4) + buffer.extend(struct.pack(" None: + append_parameter_header(buffer, pid, 4) + buffer.extend((1 if value else 0, 0, 0, 0)) + + +def append_parameter_duration(buffer: bytearray, pid: int, seconds: int, nanoseconds: int) -> None: + append_parameter_header(buffer, pid, 8) + buffer.extend(struct.pack(" bytes: + locator = bytearray(24) + struct.pack_into(">I", locator, 0, KIND_UDP_V4) + struct.pack_into(">I", locator, 4, port) + locator[20:24] = socket.inet_aton(ip_address) + return bytes(locator) + + +def append_parameter_locator(buffer: bytearray, pid: int, ip_address: str, port: int) -> None: + append_parameter_header(buffer, pid, 24) + buffer.extend(locator_bytes(ip_address, port)) + + +def append_parameter_string_cdr(buffer: bytearray, pid: int, text: str) -> None: + encoded = text.encode("utf-8") + append_parameter_header(buffer, pid, 4 + len(encoded) + 1) + buffer.extend(struct.pack(" None: + append_parameter_header(buffer, pid, 4 + len(payload)) + buffer.extend(struct.pack(" None: + append_parameter_header(buffer, PID_RELIABILITY, 12) + kind = RELIABILITY_RELIABLE if reliable else RELIABILITY_BEST_EFFORT + buffer.extend(struct.pack(" None: + append_parameter_header(buffer, PID_DURABILITY, 4) + buffer.extend(struct.pack(" None: + append_parameter_header(buffer, PID_LIVELINESS, 12) + buffer.extend(struct.pack(" None: + append_parameter_header(buffer, PID_HISTORY, 8) + buffer.extend(struct.pack(" None: + append_parameter_header(buffer, PID_KEY_HASH, 16) + buffer.extend(guid) + + +def append_parameter_sentinel(buffer: bytearray) -> None: + append_parameter_header(buffer, PID_SENTINEL, 0) + + +def build_parameter_list_payload(parameter_buffer: bytearray) -> bytes: + return PL_CDR_LE + bytes(parameter_buffer) + + +def build_data_submessage(reader_id: bytes, writer_id: bytes, sequence_number: int, payload: bytes) -> bytes: + high = sequence_number >> 32 + low = sequence_number & 0xFFFFFFFF + submessage_payload = bytearray() + submessage_payload.extend(struct.pack(" bytes: + header = RTPS_MAGIC + bytes((2, 3)) + VENDOR_ID + guid_prefix + return header + build_data_submessage(reader_id, writer_id, sequence_number, payload) + + +def parse_parameter_list(payload: bytes) -> List[tuple[int, bytes]]: + if len(payload) < 4 or payload[:4] != PL_CDR_LE: + return [] + parameters: List[tuple[int, bytes]] = [] + offset = 4 + while offset + 4 <= len(payload): + pid, length = struct.unpack_from(" len(payload): + return [] + value = payload[offset : offset + length] + parameters.append((pid, value)) + offset += length + offset += (4 - (length % 4)) & 0x3 + return parameters + + +def find_parameter(parameters: Iterable[tuple[int, bytes]], pid: int) -> Optional[bytes]: + for candidate_pid, candidate_value in parameters: + if candidate_pid == pid: + return candidate_value + return None + + +def parse_guid(value: Optional[bytes]) -> Optional[bytes]: + if value is None or len(value) != 16: + return None + return value + + +def parse_u32_le(value: Optional[bytes]) -> Optional[int]: + if value is None or len(value) < 4: + return None + return struct.unpack_from(" Optional[bool]: + if value is None or not value: + return None + return value[0] != 0 + + +def parse_cdr_string(value: Optional[bytes]) -> Optional[str]: + if value is None or len(value) < 4: + return None + length = struct.unpack_from(" len(value): + return None + raw = value[4 : 4 + length] + if raw.endswith(b"\x00"): + raw = raw[:-1] + return raw.decode("utf-8", errors="replace") + + +def parse_octet_sequence(value: Optional[bytes]) -> Optional[bytes]: + if value is None or len(value) < 4: + return None + length = struct.unpack_from(" len(value): + return None + return value[4 : 4 + length] + + +def parse_locator(value: Optional[bytes]) -> tuple[str, int]: + if value is None or len(value) != 24: + return ("0.0.0.0", 0) + kind = struct.unpack_from(">I", value, 0)[0] + if kind != KIND_UDP_V4: + return ("0.0.0.0", 0) + port = struct.unpack_from(">I", value, 4)[0] + ip_address = socket.inet_ntoa(value[20:24]) + return (ip_address, port) + + +def parse_reliability(value: Optional[bytes]) -> str: + kind = parse_u32_le(value) + return "reliable" if kind == RELIABILITY_RELIABLE else "best-effort" + + +def extract_enclave(value: Optional[bytes]) -> str: + if not value: + return "/" + text = value.decode("utf-8", errors="replace") + marker = "enclave=" + start = text.find(marker) + if start < 0: + return "/" + start += len(marker) + end = text.find(";", start) + if end < 0: + end = len(text) + return text[start:end] or "/" + + +def serialize_uint32_cdr(value: int) -> bytes: + return b"\x00\x01\x00\x00" + struct.pack(" Optional[int]: + if len(payload) < 8 or payload[:2] != b"\x00\x01": + return None + return struct.unpack_from(" None: + self.args = args + self.ports = compute_port_mapping(args.domain_id, args.participant_id) + self.guid_prefix = make_guid_prefix(args.node_name, args.domain_id, args.participant_id) + self.participant_guid = make_guid(self.guid_prefix, PARTICIPANT_ENTITY_ID) + self.sequence_numbers: Dict[bytes, int] = {} + self.discovered_participants: Dict[bytes, ParticipantProxy] = {} + self.discovered_writers: Dict[bytes, EndpointProxy] = {} + self.discovered_readers: Dict[bytes, EndpointProxy] = {} + + self.local_writers = [ + WriterConfig( + topic_name=args.publish_topic, + type_name=args.type_name, + reliable=args.reliable, + entity_index=0, + ) + ] if args.publish_topic else [] + self.local_readers = [ + ReaderConfig( + topic_name=topic_name, + type_name=args.type_name, + reliable=False, + entity_index=index, + ) + for index, topic_name in enumerate(args.subscribe_topic) + ] + + self.metatraffic_multicast_sock = self._create_metatraffic_multicast_socket() + self.metatraffic_unicast_sock = self._create_bound_udp_socket(self.ports.metatraffic_unicast) + self.user_unicast_sock = self._create_bound_udp_socket(self.ports.user_unicast) + self._configure_multicast_sender(self.metatraffic_unicast_sock) + + self.next_discovery_send = 0.0 + self.next_publish_send = 0.0 + self.last_no_participant_log = 0.0 + + def _create_bound_udp_socket(self, port: int) -> socket.socket: + sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM, socket.IPPROTO_UDP) + sock.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1) + if hasattr(socket, "SO_REUSEPORT"): + try: + sock.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEPORT, 1) + except OSError: + pass + sock.bind((self.args.bind_address, port)) + sock.setblocking(False) + return sock + + def _create_metatraffic_multicast_socket(self) -> socket.socket: + sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM, socket.IPPROTO_UDP) + sock.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1) + if hasattr(socket, "SO_REUSEPORT"): + try: + sock.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEPORT, 1) + except OSError: + pass + sock.bind(("", self.ports.metatraffic_multicast)) + interface_ip = self.args.multicast_interface or self.args.advertised_address + membership = socket.inet_aton(self.args.multicast_group) + socket.inet_aton(interface_ip) + sock.setsockopt(socket.IPPROTO_IP, socket.IP_ADD_MEMBERSHIP, membership) + sock.setblocking(False) + return sock + + def _configure_multicast_sender(self, sock: socket.socket) -> None: + sock.setsockopt(socket.IPPROTO_IP, socket.IP_MULTICAST_TTL, 1) + sock.setsockopt(socket.IPPROTO_IP, socket.IP_MULTICAST_LOOP, 1) + interface_ip = self.args.multicast_interface or self.args.advertised_address + sock.setsockopt(socket.IPPROTO_IP, socket.IP_MULTICAST_IF, socket.inet_aton(interface_ip)) + + def _next_sequence(self, writer_entity_id: bytes) -> int: + value = self.sequence_numbers.get(writer_entity_id, 1) + self.sequence_numbers[writer_entity_id] = value + 1 + return value + + def _local_writer_guid(self, entity_index: int) -> bytes: + return make_guid(self.guid_prefix, entity_id_for_index(entity_index, USER_WRITER_NO_KEY_KIND)) + + def _local_reader_guid(self, entity_index: int) -> bytes: + return make_guid(self.guid_prefix, entity_id_for_index(entity_index, USER_READER_NO_KEY_KIND)) + + def build_spdp_announce_message(self) -> bytes: + parameters = bytearray() + append_parameter_protocol_version(parameters) + append_parameter_vendor_id(parameters) + append_parameter_u32(parameters, PID_DOMAIN_ID, self.args.domain_id) + append_parameter_guid(parameters, PID_PARTICIPANT_GUID, self.participant_guid) + append_parameter_locator( + parameters, + PID_METATRAFFIC_MULTICAST_LOCATOR, + self.args.multicast_group, + self.ports.metatraffic_multicast, + ) + append_parameter_locator( + parameters, + PID_METATRAFFIC_UNICAST_LOCATOR, + self.args.advertised_address, + self.ports.metatraffic_unicast, + ) + append_parameter_locator( + parameters, + PID_DEFAULT_UNICAST_LOCATOR, + self.args.advertised_address, + self.ports.user_unicast, + ) + append_parameter_locator( + parameters, + PID_DEFAULT_MULTICAST_LOCATOR, + self.args.multicast_group, + self.ports.user_multicast, + ) + append_parameter_duration( + parameters, + PID_PARTICIPANT_LEASE_DURATION, + DEFAULT_LEASE_DURATION_SECONDS, + DEFAULT_LEASE_DURATION_NANOSECONDS, + ) + append_parameter_u32(parameters, PID_BUILTIN_ENDPOINT_SET, BUILTIN_ENDPOINT_SET) + append_parameter_octet_sequence( + parameters, + PID_USER_DATA, + f"enclave={self.args.enclave};".encode("utf-8"), + ) + append_parameter_string_cdr(parameters, PID_ENTITY_NAME, self.args.node_name) + append_parameter_sentinel(parameters) + payload = build_parameter_list_payload(parameters) + return build_rtps_message( + self.guid_prefix, + ENTITY_ID_UNKNOWN, + SPDP_WRITER_ENTITY_ID, + self._next_sequence(SPDP_WRITER_ENTITY_ID), + payload, + ) + + def build_sedp_publication_message(self, writer: WriterConfig) -> bytes: + guid = self._local_writer_guid(writer.entity_index) + parameters = bytearray() + append_parameter_guid(parameters, PID_ENDPOINT_GUID, guid) + append_parameter_locator(parameters, PID_UNICAST_LOCATOR, self.args.advertised_address, self.ports.user_unicast) + append_parameter_guid(parameters, PID_PARTICIPANT_GUID, self.participant_guid) + append_parameter_string_cdr(parameters, PID_TOPIC_NAME, writer.topic_name) + append_parameter_string_cdr(parameters, PID_TYPE_NAME, writer.type_name) + append_parameter_key_hash(parameters, guid) + append_parameter_u32(parameters, PID_TYPE_MAX_SIZE_SERIALIZED, 8) + append_parameter_protocol_version(parameters) + append_parameter_vendor_id(parameters) + append_parameter_durability(parameters) + append_parameter_liveliness(parameters) + append_parameter_reliability(parameters, writer.reliable) + append_parameter_history(parameters) + append_parameter_sentinel(parameters) + payload = build_parameter_list_payload(parameters) + return build_rtps_message( + self.guid_prefix, + SEDP_PUBLICATIONS_READER_ENTITY_ID, + SEDP_PUBLICATIONS_WRITER_ENTITY_ID, + self._next_sequence(SEDP_PUBLICATIONS_WRITER_ENTITY_ID), + payload, + ) + + def build_sedp_subscription_message(self, reader: ReaderConfig) -> bytes: + guid = self._local_reader_guid(reader.entity_index) + parameters = bytearray() + append_parameter_guid(parameters, PID_ENDPOINT_GUID, guid) + append_parameter_locator(parameters, PID_UNICAST_LOCATOR, self.args.advertised_address, self.ports.user_unicast) + append_parameter_bool(parameters, PID_EXPECTS_INLINE_QOS, False) + append_parameter_guid(parameters, PID_PARTICIPANT_GUID, self.participant_guid) + append_parameter_string_cdr(parameters, PID_TOPIC_NAME, reader.topic_name) + append_parameter_string_cdr(parameters, PID_TYPE_NAME, reader.type_name) + append_parameter_key_hash(parameters, guid) + append_parameter_protocol_version(parameters) + append_parameter_vendor_id(parameters) + append_parameter_durability(parameters) + append_parameter_liveliness(parameters) + append_parameter_reliability(parameters, reader.reliable) + append_parameter_history(parameters) + append_parameter_sentinel(parameters) + payload = build_parameter_list_payload(parameters) + return build_rtps_message( + self.guid_prefix, + SEDP_SUBSCRIPTIONS_READER_ENTITY_ID, + SEDP_SUBSCRIPTIONS_WRITER_ENTITY_ID, + self._next_sequence(SEDP_SUBSCRIPTIONS_WRITER_ENTITY_ID), + payload, + ) + + def build_uint32_data_message(self, writer: WriterConfig, value: int) -> bytes: + payload = bytearray() + payload.extend(USER_DATA_MAGIC) + payload.append(USER_DATA_VERSION) + payload.append(RELIABILITY_RELIABLE if writer.reliable else RELIABILITY_BEST_EFFORT) + topic_name = writer.topic_name.encode("utf-8") + payload.extend(struct.pack(" None: + payload = self.build_spdp_announce_message() + self.metatraffic_unicast_sock.sendto( + payload, + (self.args.multicast_group, self.ports.metatraffic_multicast), + ) + + def send_sedp_announcements_to(self, participant: ParticipantProxy) -> None: + target = (participant.address, participant.ports.metatraffic_unicast) + if participant.ports.metatraffic_unicast == 0 or not participant.address: + return + for writer in self.local_writers: + self.metatraffic_unicast_sock.sendto(self.build_sedp_publication_message(writer), target) + for reader in self.local_readers: + self.metatraffic_unicast_sock.sendto(self.build_sedp_subscription_message(reader), target) + + def send_discovery_now(self) -> None: + self.send_spdp_announce_now() + for participant in list(self.discovered_participants.values()): + self.send_sedp_announcements_to(participant) + + def publish_now(self) -> None: + if not self.local_writers: + return + if not self._publish_value(self.local_writers[0], self.args.publish_value): + now = time.monotonic() + if now - self.last_no_participant_log > 2.0: + log( + f"[publish] no discovered participants yet for topic '{self.local_writers[0].topic_name}', " + "waiting for SPDP" + ) + self.last_no_participant_log = now + else: + log( + f"[publish] sent {self.args.publish_value} on '{self.local_writers[0].topic_name}' " + f"to {len(self.discovered_participants)} discovered participant(s)" + ) + + def _publish_value(self, writer: WriterConfig, value: int, target: Optional[tuple[str, int]] = None) -> bool: + payload = self.build_uint32_data_message(writer, value) + if target is not None: + self.user_unicast_sock.sendto(payload, target) + return True + if not self.discovered_participants: + return False + for participant in self.discovered_participants.values(): + self.user_unicast_sock.sendto(payload, (participant.address, participant.ports.user_unicast)) + return True + + def handle_metatraffic_packet(self, packet: bytes, sender_ip: str) -> None: + for writer_id, serialized_payload in parse_rtps_data_messages(packet): + parameters = parse_parameter_list(serialized_payload) + if not parameters: + continue + + if writer_id == SPDP_WRITER_ENTITY_ID: + self._handle_spdp(parameters, sender_ip) + elif writer_id == SEDP_PUBLICATIONS_WRITER_ENTITY_ID: + self._handle_sedp(parameters, sender_ip, is_reader=False) + elif writer_id == SEDP_SUBSCRIPTIONS_WRITER_ENTITY_ID: + self._handle_sedp(parameters, sender_ip, is_reader=True) + + def _handle_spdp(self, parameters: List[tuple[int, bytes]], sender_ip: str) -> None: + participant_guid = parse_guid(find_parameter(parameters, PID_PARTICIPANT_GUID)) + if participant_guid is None or participant_guid[:12] == self.guid_prefix: + return + + meta_ip, meta_uc_port = parse_locator(find_parameter(parameters, PID_METATRAFFIC_UNICAST_LOCATOR)) + _, meta_mc_port = parse_locator(find_parameter(parameters, PID_METATRAFFIC_MULTICAST_LOCATOR)) + user_ip, user_uc_port = parse_locator(find_parameter(parameters, PID_DEFAULT_UNICAST_LOCATOR)) + _, user_mc_port = parse_locator(find_parameter(parameters, PID_DEFAULT_MULTICAST_LOCATOR)) + + participant = ParticipantProxy( + participant_guid=participant_guid, + guid_prefix=participant_guid[:12], + name=parse_cdr_string(find_parameter(parameters, PID_ENTITY_NAME)) or "", + enclave=extract_enclave(parse_octet_sequence(find_parameter(parameters, PID_USER_DATA))), + address=user_ip if user_ip != "0.0.0.0" else sender_ip, + ports=PortMapping( + metatraffic_multicast=meta_mc_port, + metatraffic_unicast=meta_uc_port, + user_multicast=user_mc_port, + user_unicast=user_uc_port, + ), + builtin_endpoints=parse_u32_le(find_parameter(parameters, PID_BUILTIN_ENDPOINT_SET)) or 0, + ) + + is_new = participant_guid not in self.discovered_participants + self.discovered_participants[participant_guid] = participant + label = participant.name or hex_string(participant.guid_prefix) + log( + f"[spdp] participant '{label}' at {participant.address} " + f"(meta={participant.ports.metatraffic_unicast}, user={participant.ports.user_unicast}, " + f"enclave={participant.enclave})" + ) + if is_new: + self.send_sedp_announcements_to(participant) + + def _handle_sedp(self, parameters: List[tuple[int, bytes]], sender_ip: str, is_reader: bool) -> None: + endpoint_guid = parse_guid(find_parameter(parameters, PID_ENDPOINT_GUID)) + if endpoint_guid is None or endpoint_guid[:12] == self.guid_prefix: + return + + participant_guid = parse_guid(find_parameter(parameters, PID_PARTICIPANT_GUID)) + if participant_guid is None: + participant_guid = endpoint_guid[:12] + PARTICIPANT_ENTITY_ID + + endpoint_ip, endpoint_port = parse_locator(find_parameter(parameters, PID_UNICAST_LOCATOR)) + endpoint = EndpointProxy( + guid=endpoint_guid, + participant_guid=participant_guid, + topic_name=parse_cdr_string(find_parameter(parameters, PID_TOPIC_NAME)) or "", + type_name=parse_cdr_string(find_parameter(parameters, PID_TYPE_NAME)) or "", + reliability=parse_reliability(find_parameter(parameters, PID_RELIABILITY)), + is_reader=is_reader, + expects_inline_qos=parse_bool(find_parameter(parameters, PID_EXPECTS_INLINE_QOS)) or False, + unicast_address=endpoint_ip if endpoint_ip != "0.0.0.0" else sender_ip, + unicast_port=endpoint_port, + ) + endpoint_map = self.discovered_readers if is_reader else self.discovered_writers + is_new = endpoint_guid not in endpoint_map + endpoint_map[endpoint_guid] = endpoint + if is_new: + kind = "reader" if is_reader else "writer" + log( + f"[sedp] {kind} topic='{endpoint.topic_name}' type='{endpoint.type_name}' " + f"reliability={endpoint.reliability} participant={guid_to_string(endpoint.participant_guid)}" + ) + + def handle_user_packet(self, packet: bytes, sender_ip: str, sender_port: int) -> None: + for writer_id, serialized_payload in parse_rtps_data_messages(packet): + if not serialized_payload.startswith(USER_DATA_MAGIC) or len(serialized_payload) < len(USER_DATA_MAGIC) + 2: + continue + offset = len(USER_DATA_MAGIC) + version = serialized_payload[offset] + reliability = serialized_payload[offset + 1] + offset += 2 + if version != USER_DATA_VERSION: + continue + if offset + 2 > len(serialized_payload): + continue + topic_length = struct.unpack_from(" len(serialized_payload): + continue + topic_name = serialized_payload[offset : offset + topic_length].decode("utf-8", errors="replace") + offset += topic_length + if offset + 2 > len(serialized_payload): + continue + payload_length = struct.unpack_from(" len(serialized_payload): + continue + maybe_value = deserialize_uint32_cdr(serialized_payload[offset : offset + payload_length]) + if maybe_value is None: + continue + reliability_name = "reliable" if reliability == RELIABILITY_RELIABLE else "best-effort" + log( + f"[data] topic='{topic_name}' value={maybe_value} reliability={reliability_name} " + f"from {sender_ip}:{sender_port} writer={hex_string(writer_id)}" + ) + if self.args.echo_received and self.local_writers: + subscribed_topics = {reader.topic_name for reader in self.local_readers} + if topic_name in subscribed_topics: + writer = self.local_writers[0] + self._publish_value(writer, maybe_value, (sender_ip, sender_port)) + log( + f"[echo] responded with value={maybe_value} on '{writer.topic_name}' " + f"to {sender_ip}:{sender_port}" + ) + + def run(self) -> None: + start_time = time.monotonic() + self.next_discovery_send = start_time + self.next_publish_send = start_time + self.args.publish_interval + + log( + "Starting RTPS host harness\n" + f" node: {self.args.node_name}\n" + f" advertised address: {self.args.advertised_address}\n" + f" domain/participant: {self.args.domain_id}/{self.args.participant_id}\n" + f" ports: meta_mc={self.ports.metatraffic_multicast}, meta_uc={self.ports.metatraffic_unicast}, " + f"user_mc={self.ports.user_multicast}, user_uc={self.ports.user_unicast}" + ) + if self.local_readers: + log(" readers: " + ", ".join(reader.topic_name for reader in self.local_readers)) + if self.local_writers: + writer = self.local_writers[0] + writer_mode = "echo responder" if self.args.echo_received else "periodic publisher" + interval_text = ( + f", publish value={self.args.publish_value} every {self.args.publish_interval:.2f}s" + if self.args.publish_interval > 0 + else "" + ) + log(f" writer: {writer.topic_name} ({reliability_to_name(writer.reliable)}, {writer_mode}{interval_text})") + + try: + while True: + now = time.monotonic() + if now >= self.next_discovery_send: + self.send_discovery_now() + self.next_discovery_send = now + self.args.announce_period + if self.local_writers and self.args.publish_interval > 0 and now >= self.next_publish_send: + self.publish_now() + self.next_publish_send = now + self.args.publish_interval + if self.args.duration > 0 and now - start_time >= self.args.duration: + break + + readable, _, _ = select.select( + [ + self.metatraffic_multicast_sock, + self.metatraffic_unicast_sock, + self.user_unicast_sock, + ], + [], + [], + 0.2, + ) + for sock in readable: + packet, sender = sock.recvfrom(4096) + sender_ip, sender_port = sender[0], sender[1] + if sock is self.user_unicast_sock: + self.handle_user_packet(packet, sender_ip, sender_port) + else: + self.handle_metatraffic_packet(packet, sender_ip) + except KeyboardInterrupt: + log("Stopping RTPS host harness") + finally: + self.close() + + def close(self) -> None: + for sock in ( + self.metatraffic_multicast_sock, + self.metatraffic_unicast_sock, + self.user_unicast_sock, + ): + try: + sock.close() + except OSError: + pass + + +def parse_rtps_data_messages(packet: bytes) -> List[tuple[bytes, bytes]]: + if len(packet) < 20 or not packet.startswith(RTPS_MAGIC): + return [] + offset = 20 + messages: List[tuple[bytes, bytes]] = [] + while offset + 4 <= len(packet): + kind = packet[offset] + flags = packet[offset + 1] + length = struct.unpack_from(" len(packet): + break + payload = packet[offset : offset + length] + offset += length + if kind != DATA_SUBMESSAGE_KIND or (flags & 0x04) == 0: + continue + if len(payload) < 20: + continue + extra_flags, octets_to_inline_qos = struct.unpack_from(" argparse.Namespace: + parser = argparse.ArgumentParser( + description=( + "Discover an ESPP RTPS participant from a PC/host and optionally " + "exchange temporary UInt32 user-data samples." + ) + ) + parser.add_argument("--node-name", default="python_rtps_host", help="Local participant name") + parser.add_argument("--domain-id", type=int, default=0, help="RTPS domain id") + parser.add_argument("--participant-id", type=int, default=10, help="Local participant id") + parser.add_argument("--bind-address", default="0.0.0.0", help="Local bind address") + parser.add_argument( + "--advertised-address", + default=None, + help="IPv4 address to advertise to peers (defaults to best-effort local IPv4)", + ) + parser.add_argument( + "--multicast-interface", + default=None, + help="IPv4 interface to use for multicast join/send (defaults to advertised address)", + ) + parser.add_argument("--multicast-group", default="239.255.0.1", help="RTPS metatraffic multicast group") + parser.add_argument("--enclave", default="/", help="Enclave string advertised in SPDP user data") + parser.add_argument( + "--subscribe-topic", + action="append", + default=None, + help=f"Topic name to advertise as a local reader (repeatable). Defaults to {DEFAULT_REQUEST_TOPIC}.", + ) + parser.add_argument( + "--publish-topic", + default=None, + help=f"Topic name to publish as a local writer. Defaults to {DEFAULT_RESPONSE_TOPIC}.", + ) + parser.add_argument("--publish-value", type=int, default=42, help="UInt32 value to publish") + parser.add_argument( + "--publish-interval", + type=float, + default=0.0, + help="Seconds between periodic publish attempts when --publish-topic is set (0 disables periodic publishing)", + ) + parser.set_defaults(echo_received=True) + parser.add_argument( + "--echo-received", + dest="echo_received", + action="store_true", + help="Echo received subscribed-topic values back on the publish topic (enabled by default)", + ) + parser.add_argument( + "--no-echo-received", + dest="echo_received", + action="store_false", + help="Disable request/response echo behavior and only use periodic publishing", + ) + parser.add_argument("--reliable", action="store_true", help="Mark the local writer as reliable") + parser.add_argument("--type-name", default="std_msgs/msg/UInt32", help="Advertised type name") + parser.add_argument( + "--announce-period", + type=float, + default=1.0, + help="Seconds between periodic SPDP/SEDP discovery announcements", + ) + parser.add_argument( + "--duration", + type=float, + default=0.0, + help="Stop after this many seconds (0 = run until Ctrl+C)", + ) + args = parser.parse_args() + + if args.subscribe_topic is None: + args.subscribe_topic = [DEFAULT_REQUEST_TOPIC] + if args.publish_topic is None: + args.publish_topic = DEFAULT_RESPONSE_TOPIC + if args.advertised_address is None: + args.advertised_address = guess_local_ipv4() + try: + ipaddress.IPv4Address(args.advertised_address) + ipaddress.IPv4Address(args.multicast_interface or args.advertised_address) + ipaddress.IPv4Address(args.multicast_group) + except ipaddress.AddressValueError as exc: + parser.error(str(exc)) + if args.publish_interval < 0: + parser.error("--publish-interval must be >= 0") + if args.announce_period <= 0: + parser.error("--announce-period must be > 0") + return args + + +def main() -> int: + args = parse_args() + harness = RtpsHostHarness(args) + harness.run() + return 0 + + +if __name__ == "__main__": + sys.exit(main()) From 293a9fd4e58b77ae92842c6a2f606ee69575085e Mon Sep 17 00:00:00 2001 From: William Emfinger Date: Mon, 15 Jun 2026 14:24:22 -0500 Subject: [PATCH 02/24] address comments --- components/cdr/include/cdr.hpp | 17 ++++++---- components/rtps/example/main/rtps_example.cpp | 3 +- components/rtps/src/rtps.cpp | 15 +++++---- doc/conf_common.py | 2 +- python/rtps_host.py | 32 +++++++++++++++---- 5 files changed, 47 insertions(+), 22 deletions(-) diff --git a/components/cdr/include/cdr.hpp b/components/cdr/include/cdr.hpp index a3c8a5944..b2c01bf29 100644 --- a/components/cdr/include/cdr.hpp +++ b/components/cdr/include/cdr.hpp @@ -207,12 +207,8 @@ class CdrWriter { template requires(std::is_integral_v || std::is_floating_point_v) bool write_array(const std::array &values) { - for (const auto &value : values) { - if (!write(value)) { - return false; - } - } - return true; + return std::all_of(values.begin(), values.end(), + [this](const auto &value) { return write(value); }); } /// @brief Append a variable-length CDR sequence of primitive values. @@ -376,7 +372,14 @@ class CdrReader { /// @return True if a complete value was decoded, false otherwise. template requires(std::is_integral_v || std::is_floating_point_v) bool read(T &value) { - if (!align(detail::cdr_alignment()) || remaining() < sizeof(T)) { + constexpr size_t alignment = detail::cdr_alignment(); + if constexpr (alignment > 1) { + if (!align(alignment)) { + valid_ = false; + return false; + } + } + if (remaining() < sizeof(T)) { valid_ = false; return false; } diff --git a/components/rtps/example/main/rtps_example.cpp b/components/rtps/example/main/rtps_example.cpp index 2abce874f..4786347f7 100644 --- a/components/rtps/example/main/rtps_example.cpp +++ b/components/rtps/example/main/rtps_example.cpp @@ -96,7 +96,6 @@ extern "C" void app_main(void) { std::atomic response_count{0}; std::atomic next_request_value{1}; std::atomic last_sent_request{0}; - espp::RtpsParticipant *participant_ptr = nullptr; espp::RtpsParticipant participant({ .node_name = node_name, @@ -117,7 +116,6 @@ extern "C" void app_main(void) { }, .log_level = espp::Logger::Verbosity::INFO, }); - participant_ptr = &participant; #if CONFIG_RTPS_EXAMPLE_ROLE_INITIATOR participant.add_writer({ @@ -138,6 +136,7 @@ extern "C" void app_main(void) { }, }); #else + auto *participant_ptr = &participant; participant.add_writer({ .topic_name = response_topic, .type_name = std::string(kTypeName), diff --git a/components/rtps/src/rtps.cpp b/components/rtps/src/rtps.cpp index df526118c..04ca02097 100644 --- a/components/rtps/src/rtps.cpp +++ b/components/rtps/src/rtps.cpp @@ -4,6 +4,7 @@ #include #include #include +#include #include #include @@ -1039,7 +1040,10 @@ bool RtpsParticipant::publish_uint32(std::string_view topic_name, uint32_t value logger_.warn("Reliable user-data retransmission is not implemented yet; sending best-effort"); } - auto payload = build_uint32_data_message(topic_name, value, writer_config.reliability); + auto encoded_reliability = writer_config.reliability == ReliabilityKind::RELIABLE + ? ReliabilityKind::BEST_EFFORT + : writer_config.reliability; + auto payload = build_uint32_data_message(topic_name, value, encoded_reliability); auto participants = discovered_participants(); if (participants.empty()) { logger_.warn("No discovered participants available for topic '{}'", topic_name); @@ -1393,12 +1397,11 @@ bool RtpsParticipant::send_sedp_announcements_to(const ParticipantProxy &partici } bool RtpsParticipant::send_discovery_now() { - auto sent = send_spdp_announce_now(); auto participants = discovered_participants(); - for (const auto &participant : participants) { - sent = send_sedp_announcements_to(participant) || sent; - } - return sent; + return std::accumulate(participants.begin(), participants.end(), send_spdp_announce_now(), + [this](bool sent, const auto &participant) { + return send_sedp_announcements_to(participant) || sent; + }); } RtpsParticipant::ParticipantProxy RtpsParticipant::make_local_participant_proxy() const { diff --git a/doc/conf_common.py b/doc/conf_common.py index 6cc7137d2..526875180 100644 --- a/doc/conf_common.py +++ b/doc/conf_common.py @@ -13,7 +13,7 @@ mermaid_dark_theme = 'neutral' mermaid_light_theme = 'neutral' -exclude_paterns = ['build', '_build', 'detail'] +exclude_patterns = ['build', '_build', 'detail'] # link roles config github_repo = 'esp-cpp/espp' diff --git a/python/rtps_host.py b/python/rtps_host.py index 81eef251b..ece9fc617 100644 --- a/python/rtps_host.py +++ b/python/rtps_host.py @@ -47,6 +47,8 @@ RELIABILITY_BEST_EFFORT = 1 RELIABILITY_RELIABLE = 2 +USER_DATA_RELIABILITY_BEST_EFFORT = 0 +USER_DATA_RELIABILITY_RELIABLE = 1 KIND_UDP_V4 = 1 VENDOR_ID = b"\xca\xfe" @@ -490,6 +492,8 @@ def _create_bound_udp_socket(self, port: int) -> socket.socket: try: sock.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEPORT, 1) except OSError: + # Some platforms expose SO_REUSEPORT but reject setting it; this is + # a best-effort optimization and is not required for correctness. pass sock.bind((self.args.bind_address, port)) sock.setblocking(False) @@ -502,8 +506,15 @@ def _create_metatraffic_multicast_socket(self) -> socket.socket: try: sock.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEPORT, 1) except OSError: + # Some platforms expose SO_REUSEPORT but reject setting it; this is + # a best-effort optimization and is not required for correctness. pass - sock.bind(("", self.ports.metatraffic_multicast)) + try: + sock.bind((self.args.multicast_group, self.ports.metatraffic_multicast)) + except OSError: + # Not all platforms allow binding directly to the multicast group + # address, so fall back to the selected local interface address. + sock.bind((self.args.bind_address, self.ports.metatraffic_multicast)) interface_ip = self.args.multicast_interface or self.args.advertised_address membership = socket.inet_aton(self.args.multicast_group) + socket.inet_aton(interface_ip) sock.setsockopt(socket.IPPROTO_IP, socket.IP_ADD_MEMBERSHIP, membership) @@ -636,7 +647,7 @@ def build_uint32_data_message(self, writer: WriterConfig, value: int) -> bytes: payload = bytearray() payload.extend(USER_DATA_MAGIC) payload.append(USER_DATA_VERSION) - payload.append(RELIABILITY_RELIABLE if writer.reliable else RELIABILITY_BEST_EFFORT) + payload.append(USER_DATA_RELIABILITY_RELIABLE if writer.reliable else USER_DATA_RELIABILITY_BEST_EFFORT) topic_name = writer.topic_name.encode("utf-8") payload.extend(struct.pack(" maybe_value = deserialize_uint32_cdr(serialized_payload[offset : offset + payload_length]) if maybe_value is None: continue - reliability_name = "reliable" if reliability == RELIABILITY_RELIABLE else "best-effort" + reliability_name = ( + "reliable" if reliability == USER_DATA_RELIABILITY_RELIABLE else "best-effort" + ) log( f"[data] topic='{topic_name}' value={maybe_value} reliability={reliability_name} " f"from {sender_ip}:{sender_port} writer={hex_string(writer_id)}" @@ -890,8 +903,8 @@ def close(self) -> None: ): try: sock.close() - except OSError: - pass + except OSError as exc: + log(f"[close] ignoring socket close failure for {sock!r}: {exc}") def parse_rtps_data_messages(packet: bytes) -> List[tuple[bytes, bytes]]: @@ -932,7 +945,11 @@ def parse_args() -> argparse.Namespace: parser.add_argument("--node-name", default="python_rtps_host", help="Local participant name") parser.add_argument("--domain-id", type=int, default=0, help="RTPS domain id") parser.add_argument("--participant-id", type=int, default=10, help="Local participant id") - parser.add_argument("--bind-address", default="0.0.0.0", help="Local bind address") + parser.add_argument( + "--bind-address", + default=None, + help="Local bind address (defaults to the advertised address rather than all interfaces)", + ) parser.add_argument( "--advertised-address", default=None, @@ -998,7 +1015,10 @@ def parse_args() -> argparse.Namespace: args.publish_topic = DEFAULT_RESPONSE_TOPIC if args.advertised_address is None: args.advertised_address = guess_local_ipv4() + if args.bind_address is None: + args.bind_address = args.advertised_address try: + ipaddress.IPv4Address(args.bind_address) ipaddress.IPv4Address(args.advertised_address) ipaddress.IPv4Address(args.multicast_interface or args.advertised_address) ipaddress.IPv4Address(args.multicast_group) From f9f4317ac613a843a307495ce90eb6462b6b77c8 Mon Sep 17 00:00:00 2001 From: William Emfinger Date: Mon, 15 Jun 2026 15:05:10 -0500 Subject: [PATCH 03/24] improve code --- components/cdr/include/cdr.hpp | 7 +++++-- python/rtps_host.py | 22 ++++++++++++++-------- 2 files changed, 19 insertions(+), 10 deletions(-) diff --git a/components/cdr/include/cdr.hpp b/components/cdr/include/cdr.hpp index b2c01bf29..42f3ec1f2 100644 --- a/components/cdr/include/cdr.hpp +++ b/components/cdr/include/cdr.hpp @@ -149,13 +149,16 @@ class CdrWriter { /// @brief Pad the buffer with zeros until it satisfies the requested alignment. /// @param alignment Required alignment in bytes. Values less than or equal to 1 are ignored. - void align(size_t alignment) { + /// @return Always returns true. This matches the reader API even though writer-side alignment + /// cannot currently fail. + bool align(size_t alignment) { if (alignment <= 1) { - return; + return true; } while (data_.size() % alignment != 0) { data_.push_back(0); } + return true; } /// @brief Append a primitive scalar using CDR alignment and endianness rules. diff --git a/python/rtps_host.py b/python/rtps_host.py index ece9fc617..8c30e92ec 100644 --- a/python/rtps_host.py +++ b/python/rtps_host.py @@ -45,8 +45,8 @@ DATA_SUBMESSAGE_FLAGS = 0x01 | 0x04 DATA_SUBMESSAGE_OCTETS_TO_INLINE_QOS = 16 -RELIABILITY_BEST_EFFORT = 1 -RELIABILITY_RELIABLE = 2 +RTPS_QOS_RELIABILITY_BEST_EFFORT = 1 +RTPS_QOS_RELIABILITY_RELIABLE = 2 USER_DATA_RELIABILITY_BEST_EFFORT = 0 USER_DATA_RELIABILITY_RELIABLE = 1 @@ -177,6 +177,14 @@ def reliability_to_name(reliable: bool) -> str: return "reliable" if reliable else "best-effort" +def encode_user_data_reliability(reliable: bool) -> int: + return USER_DATA_RELIABILITY_RELIABLE if reliable else USER_DATA_RELIABILITY_BEST_EFFORT + + +def decode_user_data_reliability(encoded: int) -> str: + return "reliable" if encoded == USER_DATA_RELIABILITY_RELIABLE else "best-effort" + + def compute_port_mapping(domain_id: int, participant_id: int) -> PortMapping: base = PORT_BASE + DOMAIN_GAIN * domain_id participant_offset = PARTICIPANT_GAIN * participant_id @@ -286,7 +294,7 @@ def append_parameter_octet_sequence(buffer: bytearray, pid: int, payload: bytes) def append_parameter_reliability(buffer: bytearray, reliable: bool) -> None: append_parameter_header(buffer, PID_RELIABILITY, 12) - kind = RELIABILITY_RELIABLE if reliable else RELIABILITY_BEST_EFFORT + kind = RTPS_QOS_RELIABILITY_RELIABLE if reliable else RTPS_QOS_RELIABILITY_BEST_EFFORT buffer.extend(struct.pack(" tuple[str, int]: def parse_reliability(value: Optional[bytes]) -> str: kind = parse_u32_le(value) - return "reliable" if kind == RELIABILITY_RELIABLE else "best-effort" + return "reliable" if kind == RTPS_QOS_RELIABILITY_RELIABLE else "best-effort" def extract_enclave(value: Optional[bytes]) -> str: @@ -647,7 +655,7 @@ def build_uint32_data_message(self, writer: WriterConfig, value: int) -> bytes: payload = bytearray() payload.extend(USER_DATA_MAGIC) payload.append(USER_DATA_VERSION) - payload.append(USER_DATA_RELIABILITY_RELIABLE if writer.reliable else USER_DATA_RELIABILITY_BEST_EFFORT) + payload.append(encode_user_data_reliability(writer.reliable)) topic_name = writer.topic_name.encode("utf-8") payload.extend(struct.pack(" maybe_value = deserialize_uint32_cdr(serialized_payload[offset : offset + payload_length]) if maybe_value is None: continue - reliability_name = ( - "reliable" if reliability == USER_DATA_RELIABILITY_RELIABLE else "best-effort" - ) + reliability_name = decode_user_data_reliability(reliability) log( f"[data] topic='{topic_name}' value={maybe_value} reliability={reliability_name} " f"from {sender_ip}:{sender_port} writer={hex_string(writer_id)}" From 64634d9776ed9f396f84658bba1e1c8cf110e895 Mon Sep 17 00:00:00 2001 From: William Emfinger Date: Mon, 15 Jun 2026 15:11:11 -0500 Subject: [PATCH 04/24] update readmes --- components/rtps/README.md | 54 +++++++++++++++++++++++++++++++++ components/rtsp/README.md | 64 +++++++++++++++++++++++++++++++++++++++ 2 files changed, 118 insertions(+) diff --git a/components/rtps/README.md b/components/rtps/README.md index 05ec19090..c9babda33 100644 --- a/components/rtps/README.md +++ b/components/rtps/README.md @@ -22,6 +22,60 @@ standards-shaped, but the reliable RTPS state machines (`HEARTBEAT`, `ACKNACK`, resend windows) and ROS 2 endpoint/user-data interoperability are still incomplete. +## How RTPS Works + +RTPS separates *metatraffic* from *user traffic*. + +- **Metatraffic** carries discovery and endpoint metadata. In this component, + that means SPDP participant announcements plus SEDP publication and + subscription announcements. +- **User traffic** carries application samples. The current ESPP scaffold has a + temporary best-effort `UInt32` user-data path while the standards-based + ROS 2 data plane is still being completed. + +The current `RtpsParticipant` implementation opens three UDP sockets when +`start()` is called: + +1. metatraffic multicast receive on the well-known SPDP multicast port +2. metatraffic unicast receive on the participant-specific discovery port +3. user unicast receive on the participant-specific user-data port + +It then starts a periodic announce task which multicasts SPDP and unicasts SEDP +endpoint announcements to each discovered peer. + +```mermaid +flowchart LR + App["Application code"] --> Participant["RtpsParticipant"] + Participant --> SPDP["SPDP participant DATA"] + Participant --> SEDP["SEDP publication/subscription DATA"] + Participant --> User["User DATA submessages"] + SPDP --> MetaMC["Metatraffic multicast"] + SEDP --> MetaUC["Metatraffic unicast"] + User --> UserUC["User unicast"] + MetaMC --> Peer["Remote participant"] + MetaUC --> Peer + UserUC --> Peer +``` + +## Discovery Flow + +At a high level, discovery proceeds like this: + +```mermaid +sequenceDiagram + participant A as Local participant + participant MC as 239.255.0.1 + participant B as Remote participant + A->>MC: SPDP DATA(participant GUID, locators, enclave, builtin endpoints) + MC-->>B: multicast delivery + B->>MC: SPDP DATA(its participant metadata) + MC-->>A: multicast delivery + A->>B: SEDP publication DATA(topic/type/reliability) + A->>B: SEDP subscription DATA(topic/type/reliability) + B->>A: SEDP publication/subscription DATA + Note over A,B: Matching user-data traffic can then use the user-unicast ports +``` + ## Expected Compatibility The table below is intentionally conservative: **expected** means "this is the diff --git a/components/rtsp/README.md b/components/rtsp/README.md index e6af54e4c..a5408d8eb 100755 --- a/components/rtsp/README.md +++ b/components/rtsp/README.md @@ -12,6 +12,8 @@ performed externally. **Table of Contents** - [RTSP (Real-Time Streaming Protocol) Component](#rtsp-real-time-streaming-protocol-component) + - [How RTSP Works](#how-rtsp-works) + - [Packetization Pipeline](#packetization-pipeline) - [RTSP Client](#rtsp-client) - [RTSP Server](#rtsp-server) - [Packetizers and Depacketizers](#packetizers-and-depacketizers) @@ -20,6 +22,68 @@ performed externally. +## How RTSP Works + +The component uses a split control-plane / media-plane design: + +- **RTSP over TCP** handles session control such as `OPTIONS`, `DESCRIBE`, + `SETUP`, `PLAY`, `PAUSE`, and `TEARDOWN`. +- **SDP** returned from `DESCRIBE` tells the client what tracks exist, how + they are encoded, and which per-track control URLs must be used for + `SETUP`. +- **RTP/UDP** carries encoded media packets after playback starts. +- **RTCP/UDP** sockets are created alongside RTP sockets, but the current ESPP + implementation keeps RTCP support lightweight and does not yet implement a + full control/feedback plane. + +```mermaid +sequenceDiagram + participant App as Application + participant Server as RtspServer / RtspSession + participant Client as RtspClient + App->>Server: add_track() / send_frame() + Client->>Server: OPTIONS + Server-->>Client: 200 OK + Client->>Server: DESCRIBE + Server-->>Client: SDP with session + track control paths + Client->>Server: SETUP(trackID=n, client_port=RTP-RTCP) + Server-->>Client: Session + Transport headers + Client->>Server: PLAY + Server-->>Client: 200 OK + Server-->>Client: RTP/UDP packets for each active track + Client-->>App: on_jpeg_frame() or on_frame(track_id, data) + Client->>Server: TEARDOWN + Server-->>Client: 200 OK +``` + +In ESPP, the server generates one SDP description per session, with one +`m=...` section and one `a=control:.../trackID=N` entry per registered +track. The client parses those lines during `describe()` and then issues +`SETUP` once per discovered track before calling `PLAY`. + +## Packetization Pipeline + +The codec-specific logic is intentionally separated from the RTSP core: + +```mermaid +flowchart LR + Frame["Encoded frame bytes"] --> Packetizer["Codec packetizer"] + Packetizer --> Chunks["RTP payload chunks"] + Chunks --> Header["RtspServer adds RTP headers"] + Header --> Session["RtspSession sends UDP packets"] + Session --> ClientRtp["RtspClient RTP socket"] + ClientRtp --> Depacketizer["Codec depacketizer"] + Depacketizer --> Callback["Application callback"] +``` + +`RtspServer::send_frame(track_id, data)` asks the selected packetizer to split +the encoded frame into MTU-sized chunks, adds RTP headers with track-specific +SSRC and sequence numbers, and leaves the resulting packets queued for active +sessions to transmit. On the client side, `RtspClient::handle_rtp_packet()` +parses the RTP header, uses the payload type to find the matching depacketizer, +and emits a completed frame through either `on_jpeg_frame` or the generic +`on_frame(track_id, data)` callback. + ## RTSP Client The `RtspClient` class connects to an RTSP server and receives media streams From 82bc51ef11478bb821e0b8318920114c91de948f Mon Sep 17 00:00:00 2001 From: William Emfinger Date: Mon, 15 Jun 2026 15:19:52 -0500 Subject: [PATCH 05/24] suppress cppcheck --- components/cdr/include/cdr.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/components/cdr/include/cdr.hpp b/components/cdr/include/cdr.hpp index 42f3ec1f2..c8f47a216 100644 --- a/components/cdr/include/cdr.hpp +++ b/components/cdr/include/cdr.hpp @@ -377,7 +377,7 @@ class CdrReader { requires(std::is_integral_v || std::is_floating_point_v) bool read(T &value) { constexpr size_t alignment = detail::cdr_alignment(); if constexpr (alignment > 1) { - if (!align(alignment)) { + if (!align(alignment)) { // cppcheck-suppress knownConditionTrueFalse valid_ = false; return false; } From 9ab3e86eb7562c320bff6dd8740d9de243206c73 Mon Sep 17 00:00:00 2001 From: William Emfinger Date: Tue, 16 Jun 2026 08:44:05 -0500 Subject: [PATCH 06/24] fix example docs --- components/cdr/example/main/cdr_example.cpp | 4 ++-- components/rtps/example/main/rtps_example.cpp | 2 ++ components/rtps/include/rtps.hpp | 3 +++ 3 files changed, 7 insertions(+), 2 deletions(-) diff --git a/components/cdr/example/main/cdr_example.cpp b/components/cdr/example/main/cdr_example.cpp index 6bc8662cb..4942ce15e 100644 --- a/components/cdr/example/main/cdr_example.cpp +++ b/components/cdr/example/main/cdr_example.cpp @@ -12,12 +12,11 @@ extern "C" void app_main(void) { std::array input_magic{'C', 'D', 'R', '!'}; std::array input_values{10, 20, 30}; + //! [cdr example] espp::CdrWriter writer({ .encapsulation = espp::CdrEncapsulation::CDR_LE, .include_encapsulation = true, }); - - // cdr example writer.write(42); writer.write(3.25f); writer.write_string("hello cdr"); @@ -46,6 +45,7 @@ extern "C" void app_main(void) { bool inline_ok = inline_reader.encapsulation() == espp::CdrEncapsulation::PL_CDR_LE && inline_reader.read_array(decoded_magic) && inline_reader.read_string(decoded_inline_text); + //! [cdr example] if (!ok || !inline_ok) { logger.error("Failed to decode CDR payload"); diff --git a/components/rtps/example/main/rtps_example.cpp b/components/rtps/example/main/rtps_example.cpp index 4786347f7..3b9f55913 100644 --- a/components/rtps/example/main/rtps_example.cpp +++ b/components/rtps/example/main/rtps_example.cpp @@ -70,6 +70,7 @@ bool has_endpoint(std::span endpoint extern "C" void app_main(void) { espp::Logger logger({.tag = "rtps_example", .level = espp::Logger::Verbosity::INFO}); + //! [rtps example] std::string ip_address; espp::WifiSta wifi_sta({.ssid = CONFIG_ESP_WIFI_SSID, .password = CONFIG_ESP_WIFI_PASSWORD, @@ -184,6 +185,7 @@ extern "C" void app_main(void) { logger.error("Failed to start RTPS participant"); return; } + //! [rtps example] #if CONFIG_RTPS_EXAMPLE_ROLE_INITIATOR logger.info("Initiator is waiting for a responder on the same domain/topic prefix..."); diff --git a/components/rtps/include/rtps.hpp b/components/rtps/include/rtps.hpp index 6e8a308da..f796578b5 100644 --- a/components/rtps/include/rtps.hpp +++ b/components/rtps/include/rtps.hpp @@ -19,6 +19,9 @@ namespace espp { /// Cross-platform RTPS protocol foundation built on top of the socket component. +/// +/// \section rtps_ex1 RTPS Example +/// \snippet rtps_example.cpp rtps example class RtpsParticipant : public BaseComponent { public: /// @brief Delivery semantics advertised for a writer or reader endpoint. From 95979f348d1723eecf69c60ca19bdb66929829ce Mon Sep 17 00:00:00 2001 From: William Emfinger Date: Tue, 16 Jun 2026 13:10:07 -0500 Subject: [PATCH 07/24] fix some bugs in data parsing so that examples properly communicate. add initial support for user data multicast --- components/rtps/README.md | 2 + components/rtps/example/README.md | 18 +- .../rtps/example/main/Kconfig.projbuild | 28 +- components/rtps/example/main/rtps_example.cpp | 28 +- components/rtps/include/rtps.hpp | 29 +- components/rtps/src/rtps.cpp | 260 +++++++++++++++--- doc/en/rtps.rst | 6 + python/README.md | 7 +- python/rtps_host.py | 97 ++++++- 9 files changed, 417 insertions(+), 58 deletions(-) diff --git a/components/rtps/README.md b/components/rtps/README.md index c9babda33..c07cd98a0 100644 --- a/components/rtps/README.md +++ b/components/rtps/README.md @@ -15,6 +15,7 @@ This component now includes the first real RTPS discovery slice: - SEDP publication and subscription announcements for local endpoints - parsing and tracking of discovered remote participants, writers, and readers - integration with the shared `cdr` component for CDR/PL_CDR payload handling +- optional best-effort user-data multicast transport, including endpoint-specific groups The long-term goal for this component is DDS/RTPS interoperability with ROS 2 nodes, including best-effort and reliable user-data flows. Discovery is now @@ -100,6 +101,7 @@ against every stack". | SEDP publication / subscription announce send/receive | **Implemented** | Local endpoints are announced and remote endpoints are cached. | | Participant / endpoint discovery callbacks | **Implemented** | Exposed through `on_participant_discovered` and `on_endpoint_discovered`. | | Temporary `UInt32` user-data path | **Implemented** | Uses the current ESPP-specific `ESPPDATA` payload, not a standards-based DDS sample representation. | +| Best-effort user-data multicast transport | **Implemented** | Supports shared participant-level multicast or endpoint-specific multicast locators advertised in SEDP; local readers only join the multicast groups configured for their topics. | | QoS fields emitted in discovery | **Partial** | Reliability, durability, liveliness, and history parameters are advertised in SEDP. | | QoS matching / policy enforcement | **Not implemented** | Remote QoS is parsed, but full writer/reader matching logic is still missing. | | Standards-based DDS user-data serialization | **Not implemented** | The current data path is a temporary ESPP scaffold for `std_msgs/msg/UInt32`. | diff --git a/components/rtps/example/README.md b/components/rtps/example/README.md index 11f5259ee..0c2183c98 100644 --- a/components/rtps/example/README.md +++ b/components/rtps/example/README.md @@ -26,7 +26,18 @@ For both boards: 1. Set the same `RTPS domain ID`, `Topic prefix`, `WiFi SSID`, and `WiFi password`. 2. Give each board a unique `RTPS participant ID`. -3. Optionally set distinct `Participant node name` values to make discovery logs easier to read. +3. Give each board a distinct `Participant node name` or keep the role-specific defaults. +4. If you want to exercise multicast user data, enable `Use best-effort user-data multicast` + on both boards and keep the same request/response multicast groups on each node. + +Fresh example configurations now default to: + +* initiator: participant ID `1`, node name `espp_rtps_initiator` +* responder: participant ID `2`, node name `espp_rtps_responder` + +If you are reusing an older build directory or `sdkconfig`, rerun `idf.py menuconfig` +or delete the stale generated config so the old shared defaults (`participant ID = 1`, +`node name = espp_rtps_node`) do not persist on both boards. For one board only: @@ -59,3 +70,8 @@ Expected signs of success: * both boards log RTPS participant and endpoint discovery * the initiator logs `Published request N` followed by `Received response N` * the responder logs `Received request N, sending response` + +When multicast user data is enabled, discovery still uses the normal RTPS +metatraffic sockets, but request samples are published to the configured request +multicast group and response samples are published to the configured response +multicast group. Each node only joins the group for the topic it subscribes to. diff --git a/components/rtps/example/main/Kconfig.projbuild b/components/rtps/example/main/Kconfig.projbuild index ff2656842..cdeb799f7 100644 --- a/components/rtps/example/main/Kconfig.projbuild +++ b/components/rtps/example/main/Kconfig.projbuild @@ -23,7 +23,8 @@ menu "RTPS Example Configuration" config RTPS_EXAMPLE_NODE_NAME string "Participant node name" - default "espp_rtps_node" + default "espp_rtps_initiator" if RTPS_EXAMPLE_ROLE_INITIATOR + default "espp_rtps_responder" if RTPS_EXAMPLE_ROLE_RESPONDER help Logical RTPS participant name announced during discovery. @@ -37,7 +38,8 @@ menu "RTPS Example Configuration" config RTPS_EXAMPLE_PARTICIPANT_ID int "RTPS participant ID" range 0 119 - default 1 + default 1 if RTPS_EXAMPLE_ROLE_INITIATOR + default 2 if RTPS_EXAMPLE_ROLE_RESPONDER help Each board should use a unique participant ID within the same domain. @@ -63,6 +65,28 @@ menu "RTPS Example Configuration" Period between request messages sent by the initiator after a responder has been discovered. + config RTPS_EXAMPLE_USE_USER_MULTICAST + bool "Use best-effort user-data multicast" + default n + help + If enabled, the example advertises and uses topic-specific + multicast groups for the request and response topics instead of + per-participant unicast delivery. + + config RTPS_EXAMPLE_REQUEST_MULTICAST_GROUP + string "Request topic multicast group" + default "239.255.0.11" + depends on RTPS_EXAMPLE_USE_USER_MULTICAST + help + IPv4 multicast group used for the /request topic. + + config RTPS_EXAMPLE_RESPONSE_MULTICAST_GROUP + string "Response topic multicast group" + default "239.255.0.12" + depends on RTPS_EXAMPLE_USE_USER_MULTICAST + help + IPv4 multicast group used for the /response topic. + config ESP_WIFI_SSID string "WiFi SSID" default "" diff --git a/components/rtps/example/main/rtps_example.cpp b/components/rtps/example/main/rtps_example.cpp index 3b9f55913..6cea5bdc4 100644 --- a/components/rtps/example/main/rtps_example.cpp +++ b/components/rtps/example/main/rtps_example.cpp @@ -58,8 +58,8 @@ bool run_local_protocol_checks(espp::Logger &logger, const espp::RtpsParticipant return true; } -bool has_endpoint(std::span endpoints, - std::string_view topic_name, bool is_reader) { +[[maybe_unused]] bool has_endpoint(std::span endpoints, + std::string_view topic_name, bool is_reader) { return std::any_of(endpoints.begin(), endpoints.end(), [topic_name, is_reader](const auto &endpoint) { return endpoint.topic_name == topic_name && endpoint.is_reader == is_reader; @@ -92,6 +92,22 @@ extern "C" void app_main(void) { const std::string topic_prefix = CONFIG_RTPS_EXAMPLE_TOPIC_PREFIX; const std::string request_topic = topic_prefix + "/request"; const std::string response_topic = topic_prefix + "/response"; +#if CONFIG_RTPS_EXAMPLE_USE_USER_MULTICAST +#if defined(CONFIG_RTPS_EXAMPLE_REQUEST_MULTICAST_GROUP) && \ + defined(CONFIG_RTPS_EXAMPLE_RESPONSE_MULTICAST_GROUP) + const std::string request_multicast_group = CONFIG_RTPS_EXAMPLE_REQUEST_MULTICAST_GROUP; + const std::string response_multicast_group = CONFIG_RTPS_EXAMPLE_RESPONSE_MULTICAST_GROUP; +#elif defined(CONFIG_RTPS_EXAMPLE_USER_MULTICAST_GROUP) + const std::string request_multicast_group = CONFIG_RTPS_EXAMPLE_USER_MULTICAST_GROUP; + const std::string response_multicast_group = CONFIG_RTPS_EXAMPLE_USER_MULTICAST_GROUP; +#else + const std::string request_multicast_group = "239.255.0.11"; + const std::string response_multicast_group = "239.255.0.12"; +#endif +#else + const std::string request_multicast_group; + const std::string response_multicast_group; +#endif std::atomic request_count{0}; std::atomic response_count{0}; @@ -123,12 +139,14 @@ extern "C" void app_main(void) { .topic_name = request_topic, .type_name = std::string(kTypeName), .reliability = espp::RtpsParticipant::ReliabilityKind::BEST_EFFORT, + .multicast_group = request_multicast_group, .entity_index = 0, }); participant.add_reader({ .topic_name = response_topic, .type_name = std::string(kTypeName), .reliability = espp::RtpsParticipant::ReliabilityKind::BEST_EFFORT, + .multicast_group = response_multicast_group, .entity_index = 0, .on_uint32_sample = [&logger, &response_count, &last_sent_request](uint32_t value) { @@ -142,12 +160,14 @@ extern "C" void app_main(void) { .topic_name = response_topic, .type_name = std::string(kTypeName), .reliability = espp::RtpsParticipant::ReliabilityKind::BEST_EFFORT, + .multicast_group = response_multicast_group, .entity_index = 0, }); participant.add_reader({ .topic_name = request_topic, .type_name = std::string(kTypeName), .reliability = espp::RtpsParticipant::ReliabilityKind::BEST_EFFORT, + .multicast_group = request_multicast_group, .entity_index = 0, .on_uint32_sample = [&logger, &request_count, &response_topic, &participant_ptr](uint32_t value) { @@ -176,6 +196,10 @@ extern "C" void app_main(void) { logger.info("Request topic: {}, Response topic: {}", request_topic, response_topic); logger.info("Ports: meta mc={}, meta uc={}, user mc={}, user uc={}", ports.metatraffic_multicast, ports.metatraffic_unicast, ports.user_multicast, ports.user_unicast); +#if CONFIG_RTPS_EXAMPLE_USE_USER_MULTICAST + logger.info("User-data multicast enabled: request group {}, response group {}", + request_multicast_group, response_multicast_group); +#endif if (!run_local_protocol_checks(logger, participant)) { return; diff --git a/components/rtps/include/rtps.hpp b/components/rtps/include/rtps.hpp index f796578b5..e861cfab8 100644 --- a/components/rtps/include/rtps.hpp +++ b/components/rtps/include/rtps.hpp @@ -166,7 +166,9 @@ class RtpsParticipant : public BaseComponent { std::string type_name{"std_msgs/msg/UInt32"}; ///< Type name advertised through SEDP. ReliabilityKind reliability{ ReliabilityKind::BEST_EFFORT}; ///< Reliability QoS advertised for the writer. - uint32_t entity_index{0}; ///< Local entity slot used to derive the RTPS entity ID. + std::string multicast_group{}; ///< Optional multicast group advertised for this writer and used + ///< by `publish_uint32()` when set. + uint32_t entity_index{0}; ///< Local entity slot used to derive the RTPS entity ID. }; /// @brief Configuration for a locally advertised reader endpoint. @@ -175,7 +177,9 @@ class RtpsParticipant : public BaseComponent { std::string type_name{"std_msgs/msg/UInt32"}; ///< Type name advertised through SEDP. ReliabilityKind reliability{ ReliabilityKind::BEST_EFFORT}; ///< Reliability QoS advertised for the reader. - uint32_t entity_index{0}; ///< Local entity slot used to derive the RTPS entity ID. + std::string multicast_group{}; ///< Optional multicast group advertised for this reader and + ///< joined on the standard RTPS user-multicast port when set. + uint32_t entity_index{0}; ///< Local entity slot used to derive the RTPS entity ID. std::function on_uint32_sample{ nullptr}; ///< Callback invoked when a matching temporary UInt32 sample is received. }; @@ -201,6 +205,8 @@ class RtpsParticipant : public BaseComponent { bool is_reader{false}; ///< True for discovered readers, false for discovered writers. bool expects_inline_qos{false}; ///< Whether the remote endpoint requested inline QoS. Locator unicast_locator{}; ///< Preferred unicast locator advertised by the endpoint. + std::vector multicast_locators{}; ///< Multicast locators advertised by the endpoint + ///< for user-data traffic. }; /// @brief Top-level participant configuration. @@ -213,6 +219,10 @@ class RtpsParticipant : public BaseComponent { "127.0.0.1"}; ///< IPv4 address advertised to peers for unicast traffic. std::string metatraffic_multicast_group{ "239.255.0.1"}; ///< Multicast group used for RTPS metatraffic discovery. + std::string user_multicast_group{ + "239.255.0.1"}; ///< Multicast group used for best-effort user-data multicast when enabled. + bool use_multicast_for_user_data{false}; ///< If true, join the user multicast group and publish + ///< temporary user-data samples via multicast. Task::BaseConfig receive_task_config{ .name = "RtpsRx", .stack_size_bytes = 6 * 1024}; ///< Base task configuration for receive sockets. @@ -317,14 +327,14 @@ class RtpsParticipant : public BaseComponent { std::vector build_sedp_subscription_message(const ReaderConfig &reader_config) const; /// @brief Build a temporary ESPP UInt32 user-data message. - /// @param topic_name Topic name to embed in the message payload. + /// @param writer_config Local writer configuration used for the topic and writer entity ID. /// @param value UInt32 sample value to serialize. /// @param reliability Reliability flag to encode in the temporary payload header. /// @return Serialized RTPS DATA message containing the temporary ESPP UInt32 payload. - std::vector build_uint32_data_message(std::string_view topic_name, uint32_t value, + std::vector build_uint32_data_message(const WriterConfig &writer_config, uint32_t value, ReliabilityKind reliability) const; - /// @brief Publish a temporary ESPP UInt32 sample to discovered participants. + /// @brief Publish a temporary ESPP UInt32 sample using the configured user-data transport. /// @param topic_name Topic name to publish on. Must match a registered local writer. /// @param value UInt32 sample value to send. /// @return True if at least one send call succeeded, false otherwise. @@ -347,8 +357,16 @@ class RtpsParticipant : public BaseComponent { static PortMapping compute_port_mapping(uint16_t domain_id, uint16_t participant_id); private: + struct UserMulticastReceiver { + std::string multicast_group{}; + std::unique_ptr socket{}; + }; + bool handle_metatraffic_message(std::vector &data, const Socket::Info &sender); bool handle_user_message(std::vector &data, const Socket::Info &sender); + bool ensure_user_multicast_receivers_started(); + std::vector + build_user_send_configs(std::string_view topic_name, const WriterConfig &writer_config) const; bool send_spdp_announce_now(); bool send_sedp_announcements_to(const ParticipantProxy &participant); bool send_discovery_now(); @@ -360,6 +378,7 @@ class RtpsParticipant : public BaseComponent { std::unique_ptr metatraffic_multicast_receiver_; std::unique_ptr metatraffic_unicast_receiver_; + std::vector user_multicast_receivers_; std::unique_ptr user_unicast_receiver_; std::unique_ptr announce_task_; diff --git a/components/rtps/src/rtps.cpp b/components/rtps/src/rtps.cpp index 04ca02097..83bf00d8d 100644 --- a/components/rtps/src/rtps.cpp +++ b/components/rtps/src/rtps.cpp @@ -474,6 +474,17 @@ std::optional find_parameter(std::span param return *iterator; } +std::vector find_parameters(std::span parameters, + ParameterId id) { + std::vector matches; + for (const auto ¶meter : parameters) { + if (parameter.id == id) { + matches.push_back(parameter); + } + } + return matches; +} + std::optional parse_guid(std::span value) { if (value.size() != 16) { return std::nullopt; @@ -506,11 +517,15 @@ std::optional parse_cdr_string(std::span value) { if (!reader.valid()) { return std::nullopt; } - std::string text; - if (!reader.read_string(text)) { + uint32_t length = 0; + if (!reader.read(length) || length == 0) { + return std::nullopt; + } + auto text_bytes = reader.read_span(length); + if (text_bytes.size() != length || text_bytes.back() != 0) { return std::nullopt; } - return text; + return std::string(reinterpret_cast(text_bytes.data()), text_bytes.size() - 1); } std::optional> parse_octet_sequence(std::span value) { @@ -546,6 +561,10 @@ std::optional parse_locator(std::span parse_reliability(std::span value) { auto maybe_kind = parse_u32_le(value); @@ -817,6 +836,11 @@ bool RtpsParticipant::start() { return false; } + if (!ensure_user_multicast_receivers_started()) { + stop(); + return false; + } + announce_task_ = Task::make_unique({ .callback = [this](std::mutex &mutex, std::condition_variable &cv, bool ¬ified) -> bool { send_discovery_now(); @@ -848,6 +872,12 @@ void RtpsParticipant::stop() { metatraffic_unicast_receiver_->stop_receiving(); metatraffic_unicast_receiver_.reset(); } + for (auto &receiver : user_multicast_receivers_) { + if (receiver.socket) { + receiver.socket->stop_receiving(); + } + } + user_multicast_receivers_.clear(); if (user_unicast_receiver_) { user_unicast_receiver_->stop_receiving(); user_unicast_receiver_.reset(); @@ -863,8 +893,15 @@ bool RtpsParticipant::add_writer(const WriterConfig &writer_config) { } bool RtpsParticipant::add_reader(const ReaderConfig &reader_config) { - std::lock_guard lock(mutex_); - readers_.push_back(reader_config); + { + std::lock_guard lock(mutex_); + readers_.push_back(reader_config); + } + if (started_.load() && !reader_config.multicast_group.empty() && + !ensure_user_multicast_receivers_started()) { + logger_.error("Failed to start multicast receiver for topic '{}'", reader_config.topic_name); + return false; + } return true; } @@ -929,9 +966,10 @@ std::vector RtpsParticipant::build_spdp_announce_message() const { Locator::udp_v4(config_.advertised_address, ports().metatraffic_unicast)); append_parameter_locator(parameters, ParameterId::PID_DEFAULT_UNICAST_LOCATOR, Locator::udp_v4(config_.advertised_address, ports().user_unicast)); - append_parameter_locator( - parameters, ParameterId::PID_DEFAULT_MULTICAST_LOCATOR, - Locator::udp_v4(config_.metatraffic_multicast_group, ports().user_multicast)); + if (config_.use_multicast_for_user_data) { + append_parameter_locator(parameters, ParameterId::PID_DEFAULT_MULTICAST_LOCATOR, + Locator::udp_v4(config_.user_multicast_group, ports().user_multicast)); + } append_parameter_duration(parameters, ParameterId::PID_PARTICIPANT_LEASE_DURATION, kDefaultLeaseDurationSeconds, kDefaultLeaseDurationNanoseconds); append_parameter_u32(parameters, ParameterId::PID_BUILTIN_ENDPOINT_SET, kBuiltinEndpointSet); @@ -956,6 +994,11 @@ RtpsParticipant::build_sedp_publication_message(const WriterConfig &writer_confi append_parameter_guid(parameters, ParameterId::PID_ENDPOINT_GUID, guid); append_parameter_locator(parameters, ParameterId::PID_UNICAST_LOCATOR, Locator::udp_v4(config_.advertised_address, ports().user_unicast)); + if (!writer_config.multicast_group.empty()) { + append_parameter_locator( + parameters, ParameterId::PID_MULTICAST_LOCATOR, + Locator::udp_v4(writer_config.multicast_group, ports().user_multicast)); + } append_parameter_guid(parameters, ParameterId::PID_PARTICIPANT_GUID, participant_guid()); append_parameter_string_cdr(parameters, ParameterId::PID_TOPIC_NAME, writer_config.topic_name); append_parameter_string_cdr(parameters, ParameterId::PID_TYPE_NAME, writer_config.type_name); @@ -984,6 +1027,11 @@ RtpsParticipant::build_sedp_subscription_message(const ReaderConfig &reader_conf append_parameter_guid(parameters, ParameterId::PID_ENDPOINT_GUID, guid); append_parameter_locator(parameters, ParameterId::PID_UNICAST_LOCATOR, Locator::udp_v4(config_.advertised_address, ports().user_unicast)); + if (!reader_config.multicast_group.empty()) { + append_parameter_locator( + parameters, ParameterId::PID_MULTICAST_LOCATOR, + Locator::udp_v4(reader_config.multicast_group, ports().user_multicast)); + } append_parameter_bool(parameters, ParameterId::PID_EXPECTS_INLINE_QOS, false); append_parameter_guid(parameters, ParameterId::PID_PARTICIPANT_GUID, participant_guid()); append_parameter_string_cdr(parameters, ParameterId::PID_TOPIC_NAME, reader_config.topic_name); @@ -1004,19 +1052,19 @@ RtpsParticipant::build_sedp_subscription_message(const ReaderConfig &reader_conf .serialize(); } -std::vector RtpsParticipant::build_uint32_data_message(std::string_view topic_name, +std::vector RtpsParticipant::build_uint32_data_message(const WriterConfig &writer_config, uint32_t value, ReliabilityKind reliability) const { ByteWriter payload_writer; payload_writer.append_chars(kUserDataMagic); payload_writer.append_u8(kUserDataVersion); payload_writer.append_u8(static_cast(reliability)); - append_string(payload_writer, topic_name); + append_string(payload_writer, writer_config.topic_name); auto cdr = serialize_uint32_cdr(value); payload_writer.append_u16_le(static_cast(cdr.size())); payload_writer.append_bytes(cdr); - auto guid = writers_.empty() ? writer_guid(0) : writer_guid(writers_.front().entity_index); + auto guid = writer_guid(writer_config.entity_index); return build_message(guid_prefix_, {.value = kEntityIdUnknown}, guid.entity_id, 1, payload_writer.take()) .serialize(); @@ -1043,23 +1091,20 @@ bool RtpsParticipant::publish_uint32(std::string_view topic_name, uint32_t value auto encoded_reliability = writer_config.reliability == ReliabilityKind::RELIABLE ? ReliabilityKind::BEST_EFFORT : writer_config.reliability; - auto payload = build_uint32_data_message(topic_name, value, encoded_reliability); - auto participants = discovered_participants(); - if (participants.empty()) { - logger_.warn("No discovered participants available for topic '{}'", topic_name); + auto payload = build_uint32_data_message(writer_config, value, encoded_reliability); + + if (!user_unicast_receiver_) { return false; } - if (!user_unicast_receiver_) { + auto send_configs = build_user_send_configs(topic_name, writer_config); + if (send_configs.empty()) { + logger_.warn("No send destinations available for topic '{}'", topic_name); return false; } bool sent = false; - for (const auto &participant : participants) { - auto send_config = UdpSocket::SendConfig{ - .ip_address = participant.address, - .port = participant.ports.user_unicast, - }; + for (const auto &send_config : send_configs) { sent = user_unicast_receiver_->send(payload, send_config) || sent; } return sent; @@ -1192,12 +1237,12 @@ bool RtpsParticipant::handle_metatraffic_message(std::vector &data, callback = config_.on_participant_discovered; } - logger_.info("SPDP discovered participant '{}' at {} (meta {}, user {})", - participant.name.empty() ? participant.guid_prefix.to_string() - : participant.name, - participant.address, participant.ports.metatraffic_unicast, - participant.ports.user_unicast); if (is_new_participant) { + logger_.info("SPDP discovered participant '{}' at {} (meta {}, user {})", + participant.name.empty() ? participant.guid_prefix.to_string() + : participant.name, + participant.address, participant.ports.metatraffic_unicast, + participant.ports.user_unicast); send_sedp_announcements_to(participant); if (callback) { callback(participant); @@ -1254,6 +1299,12 @@ bool RtpsParticipant::handle_metatraffic_message(std::vector &data, endpoint.unicast_locator = *maybe_locator; } } + for (const auto &locator_parameter : + find_parameters(parameters, ParameterId::PID_MULTICAST_LOCATOR)) { + if (auto maybe_locator = parse_locator(locator_parameter.value)) { + endpoint.multicast_locators.push_back(*maybe_locator); + } + } if (auto maybe_reliability_parameter = find_parameter(parameters, ParameterId::PID_RELIABILITY)) { if (auto maybe_reliability = parse_reliability(maybe_reliability_parameter->value)) { @@ -1284,11 +1335,13 @@ bool RtpsParticipant::handle_metatraffic_message(std::vector &data, endpoint_callback = config_.on_endpoint_discovered; } - logger_.info("SEDP discovered {} '{}' [{}] from participant {}", - endpoint.is_reader ? "reader" : "writer", endpoint.topic_name, endpoint.type_name, - endpoint.participant_guid.to_string()); - if (is_new_endpoint && endpoint_callback) { - endpoint_callback(endpoint); + if (is_new_endpoint) { + logger_.info("SEDP discovered {} '{}' [{}] from participant {}", + endpoint.is_reader ? "reader" : "writer", endpoint.topic_name, + endpoint.type_name, endpoint.participant_guid.to_string()); + if (endpoint_callback) { + endpoint_callback(endpoint); + } } } return false; @@ -1299,16 +1352,20 @@ bool RtpsParticipant::handle_user_message(std::vector &data, const Sock if (!message) { return false; } + if (message->header.guid_prefix == guid_prefix_) { + return false; + } for (const auto &submessage : message->submessages) { - if (submessage.kind != SubmessageKind::DATA || - submessage.payload.size() < kUserDataMagic.size() + 2 || - !std::equal(kUserDataMagic.begin(), kUserDataMagic.end(), submessage.payload.begin())) { + bool valid_data = false; + auto data_view = parse_data_submessage(submessage, valid_data); + if (!valid_data || data_view.serialized_payload.size() < kUserDataMagic.size() + 2 || + !std::equal(kUserDataMagic.begin(), kUserDataMagic.end(), + data_view.serialized_payload.begin())) { continue; } - ByteReader reader( - std::span{submessage.payload.data(), submessage.payload.size()}); + ByteReader reader(data_view.serialized_payload); std::array magic{}; uint8_t version = 0; uint8_t reliability = 0; @@ -1349,6 +1406,137 @@ bool RtpsParticipant::handle_user_message(std::vector &data, const Sock return false; } +bool RtpsParticipant::ensure_user_multicast_receivers_started() { + if (!started_.load()) { + return true; + } + + std::vector desired_groups; + if (config_.use_multicast_for_user_data && !config_.user_multicast_group.empty()) { + desired_groups.push_back(config_.user_multicast_group); + } + { + std::lock_guard lock(mutex_); + for (const auto &reader_config : readers_) { + if (!reader_config.multicast_group.empty() && + std::find(desired_groups.begin(), desired_groups.end(), reader_config.multicast_group) == + desired_groups.end()) { + desired_groups.push_back(reader_config.multicast_group); + } + } + } + + auto port_mapping = ports(); + for (const auto &group : desired_groups) { + auto existing = + std::find_if(user_multicast_receivers_.begin(), user_multicast_receivers_.end(), + [&group](const auto &receiver) { return receiver.multicast_group == group; }); + if (existing != user_multicast_receivers_.end()) { + continue; + } + + auto socket = std::make_unique(UdpSocket::Config{.log_level = get_log_level()}); + auto task_config = config_.receive_task_config; + task_config.name = fmt::format("{}_user_mc_{}", config_.receive_task_config.name, + user_multicast_receivers_.size()); + auto receive_config = UdpSocket::ReceiveConfig{ + .port = port_mapping.user_multicast, + .buffer_size = 4096, + .is_multicast_endpoint = true, + .multicast_group = group, + .on_receive_callback = [this](auto &data, + const auto &sender) -> std::optional> { + handle_user_message(data, sender); + return std::nullopt; + }, + }; + if (!socket->start_receiving(task_config, receive_config)) { + logger_.error("Failed to start user multicast receiver for group {}", group); + return false; + } + user_multicast_receivers_.push_back({ + .multicast_group = group, + .socket = std::move(socket), + }); + } + return true; +} + +std::vector +RtpsParticipant::build_user_send_configs(std::string_view topic_name, + const WriterConfig &writer_config) const { + std::vector send_configs; + auto add_send_config = [&send_configs](std::string ip_address, uint16_t port, bool is_multicast) { + if (ip_address.empty() || port == 0) { + return; + } + auto existing = + std::find_if(send_configs.begin(), send_configs.end(), [&](const auto &send_config) { + return send_config.ip_address == ip_address && send_config.port == port && + send_config.is_multicast_endpoint == is_multicast; + }); + if (existing == send_configs.end()) { + send_configs.push_back({ + .ip_address = std::move(ip_address), + .port = port, + .is_multicast_endpoint = is_multicast, + }); + } + }; + + if (!writer_config.multicast_group.empty()) { + add_send_config(writer_config.multicast_group, ports().user_multicast, true); + return send_configs; + } + + if (config_.use_multicast_for_user_data) { + add_send_config(config_.user_multicast_group, ports().user_multicast, true); + return send_configs; + } + + std::vector remote_readers; + std::vector participants; + { + std::lock_guard lock(mutex_); + remote_readers = discovered_readers_; + participants = discovered_participants_; + } + + for (const auto &reader : remote_readers) { + if (!reader.is_reader || reader.topic_name != topic_name) { + continue; + } + + bool used_multicast = false; + for (const auto &locator : reader.multicast_locators) { + if (!has_valid_locator(locator)) { + continue; + } + add_send_config(locator.address_string(), static_cast(locator.port), true); + used_multicast = true; + } + if (used_multicast) { + continue; + } + + if (has_valid_locator(reader.unicast_locator)) { + add_send_config(reader.unicast_locator.address_string(), + static_cast(reader.unicast_locator.port), false); + continue; + } + + auto participant = + std::find_if(participants.begin(), participants.end(), [&](const auto &proxy) { + return proxy.guid_prefix == reader.participant_guid.prefix; + }); + if (participant != participants.end()) { + add_send_config(participant->address, participant->ports.user_unicast, false); + } + } + + return send_configs; +} + bool RtpsParticipant::send_spdp_announce_now() { if (!metatraffic_unicast_receiver_) { return false; diff --git a/doc/en/rtps.rst b/doc/en/rtps.rst index a5da1cc60..1762e4485 100644 --- a/doc/en/rtps.rst +++ b/doc/en/rtps.rst @@ -17,6 +17,7 @@ This version now implements the first RTPS discovery layer on top of the ESPP - integration with the shared ``cdr`` component for CDR/PL_CDR payload handling - a participant transport layer that uses ``UdpSocket`` for metatraffic and user-traffic channels +- optional best-effort user-data multicast transport, including endpoint-specific groups The long-term target is interoperability with ROS 2 nodes over DDS/RTPS, including best-effort and reliable data flows. Discovery messages are now @@ -197,6 +198,11 @@ Feature Status - **Implemented** - Uses the current ESPP-specific ``ESPPDATA`` payload, not a standards-based DDS sample representation. + * - Best-effort user-data multicast transport + - **Implemented** + - Supports shared participant-level multicast or endpoint-specific + multicast locators advertised in SEDP; local readers only join the + multicast groups configured for their topics. * - QoS fields emitted in discovery - **Partial** - Reliability, durability, liveliness, and history parameters are diff --git a/python/README.md b/python/README.md index 429c5653f..9ea454864 100644 --- a/python/README.md +++ b/python/README.md @@ -50,7 +50,8 @@ This section gives a brief overview of what the scripts in this folder do. - `rtps_host.py`: A pure-stdlib host-side RTPS harness for discovering an ESPP `RtpsParticipant`, printing SPDP/SEDP metadata, and optionally publishing or receiving the current temporary `UInt32` user-data payloads without needing - Python bindings. + Python bindings. It now follows endpoint-advertised user-data multicast + locators, joining matching subscribed-topic multicast groups dynamically. - `cobs_demo.py`: Demonstration of ESPP COBS functionality with native Python data types. Shows ESPP encoding/decoding, cross-library compatibility with the cobs-python library, and practical usage examples. Includes design differences explanation and validation @@ -130,6 +131,10 @@ For the default ESP RTPS example, the host harness now defaults to the python3 rtps_host.py --advertised-address 192.168.1.50 ``` +When the ESP RTPS example is configured for per-topic multicast, the host +harness will automatically join the discovered request-topic multicast group and +send response samples using the discovered response-reader locators. + To act as the initiator instead, swap the topics and enable periodic publishing: ```console diff --git a/python/rtps_host.py b/python/rtps_host.py index 8c30e92ec..0aabadb38 100644 --- a/python/rtps_host.py +++ b/python/rtps_host.py @@ -25,7 +25,7 @@ import sys import time from dataclasses import dataclass -from typing import Dict, Iterable, List, Optional +from typing import Dict, Iterable, List, Optional, Set, Tuple RTPS_MAGIC = b"RTPS" @@ -86,6 +86,7 @@ PID_LIVELINESS = 0x001B PID_DURABILITY = 0x001D PID_USER_DATA = 0x002C +PID_MULTICAST_LOCATOR = 0x0030 PID_UNICAST_LOCATOR = 0x002F PID_DEFAULT_UNICAST_LOCATOR = 0x0031 PID_METATRAFFIC_UNICAST_LOCATOR = 0x0032 @@ -139,6 +140,7 @@ class EndpointProxy: expects_inline_qos: bool unicast_address: str unicast_port: int + multicast_locators: List[Tuple[str, int]] @dataclass @@ -375,6 +377,10 @@ def find_parameter(parameters: Iterable[tuple[int, bytes]], pid: int) -> Optiona return None +def find_parameters(parameters: Iterable[tuple[int, bytes]], pid: int) -> List[bytes]: + return [candidate_value for candidate_pid, candidate_value in parameters if candidate_pid == pid] + + def parse_guid(value: Optional[bytes]) -> Optional[bytes]: if value is None or len(value) != 16: return None @@ -465,6 +471,7 @@ def __init__(self, args: argparse.Namespace) -> None: self.discovered_participants: Dict[bytes, ParticipantProxy] = {} self.discovered_writers: Dict[bytes, EndpointProxy] = {} self.discovered_readers: Dict[bytes, EndpointProxy] = {} + self.joined_user_multicast_groups: Set[str] = set() self.local_writers = [ WriterConfig( @@ -487,7 +494,9 @@ def __init__(self, args: argparse.Namespace) -> None: self.metatraffic_multicast_sock = self._create_metatraffic_multicast_socket() self.metatraffic_unicast_sock = self._create_bound_udp_socket(self.ports.metatraffic_unicast) self.user_unicast_sock = self._create_bound_udp_socket(self.ports.user_unicast) + self.user_multicast_sock = self._create_user_multicast_socket() self._configure_multicast_sender(self.metatraffic_unicast_sock) + self._configure_multicast_sender(self.user_unicast_sock) self.next_discovery_send = 0.0 self.next_publish_send = 0.0 @@ -529,6 +538,27 @@ def _create_metatraffic_multicast_socket(self) -> socket.socket: sock.setblocking(False) return sock + def _create_user_multicast_socket(self) -> socket.socket: + sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM, socket.IPPROTO_UDP) + sock.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1) + if hasattr(socket, "SO_REUSEPORT"): + try: + sock.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEPORT, 1) + except OSError: + pass + sock.bind((self.args.bind_address, self.ports.user_multicast)) + sock.setblocking(False) + return sock + + def _join_user_multicast_group(self, group: str) -> None: + if group in self.joined_user_multicast_groups: + return + interface_ip = self.args.multicast_interface or self.args.advertised_address + membership = socket.inet_aton(group) + socket.inet_aton(interface_ip) + self.user_multicast_sock.setsockopt(socket.IPPROTO_IP, socket.IP_ADD_MEMBERSHIP, membership) + self.joined_user_multicast_groups.add(group) + log(f"[multicast] joined user-data group {group}:{self.ports.user_multicast}") + def _configure_multicast_sender(self, sock: socket.socket) -> None: sock.setsockopt(socket.IPPROTO_IP, socket.IP_MULTICAST_TTL, 1) sock.setsockopt(socket.IPPROTO_IP, socket.IP_MULTICAST_LOOP, 1) @@ -706,18 +736,42 @@ def publish_now(self) -> None: else: log( f"[publish] sent {self.args.publish_value} on '{self.local_writers[0].topic_name}' " - f"to {len(self.discovered_participants)} discovered participant(s)" + f"using {len(self._build_user_targets(self.local_writers[0]))} discovered target(s)" ) + def _build_user_targets(self, writer: WriterConfig) -> List[Tuple[str, int]]: + targets: List[Tuple[str, int]] = [] + for reader in self.discovered_readers.values(): + if reader.topic_name != writer.topic_name: + continue + if reader.multicast_locators: + for multicast_address, multicast_port in reader.multicast_locators: + target = (multicast_address, multicast_port) + if target not in targets: + targets.append(target) + continue + if reader.unicast_port > 0 and reader.unicast_address: + target = (reader.unicast_address, reader.unicast_port) + if target not in targets: + targets.append(target) + if targets: + return targets + for participant in self.discovered_participants.values(): + target = (participant.address, participant.ports.user_unicast) + if participant.address and participant.ports.user_unicast > 0 and target not in targets: + targets.append(target) + return targets + def _publish_value(self, writer: WriterConfig, value: int, target: Optional[tuple[str, int]] = None) -> bool: payload = self.build_uint32_data_message(writer, value) if target is not None: self.user_unicast_sock.sendto(payload, target) return True - if not self.discovered_participants: + targets = self._build_user_targets(writer) + if not targets: return False - for participant in self.discovered_participants.values(): - self.user_unicast_sock.sendto(payload, (participant.address, participant.ports.user_unicast)) + for destination in targets: + self.user_unicast_sock.sendto(payload, destination) return True def handle_metatraffic_packet(self, packet: bytes, sender_ip: str) -> None: @@ -779,6 +833,15 @@ def _handle_sedp(self, parameters: List[tuple[int, bytes]], sender_ip: str, is_r participant_guid = endpoint_guid[:12] + PARTICIPANT_ENTITY_ID endpoint_ip, endpoint_port = parse_locator(find_parameter(parameters, PID_UNICAST_LOCATOR)) + multicast_locators = [ + parse_locator(value) + for value in find_parameters(parameters, PID_MULTICAST_LOCATOR) + ] + multicast_locators = [ + (multicast_address, multicast_port) + for multicast_address, multicast_port in multicast_locators + if multicast_address != "0.0.0.0" and multicast_port > 0 + ] endpoint = EndpointProxy( guid=endpoint_guid, participant_guid=participant_guid, @@ -789,10 +852,17 @@ def _handle_sedp(self, parameters: List[tuple[int, bytes]], sender_ip: str, is_r expects_inline_qos=parse_bool(find_parameter(parameters, PID_EXPECTS_INLINE_QOS)) or False, unicast_address=endpoint_ip if endpoint_ip != "0.0.0.0" else sender_ip, unicast_port=endpoint_port, + multicast_locators=multicast_locators, ) endpoint_map = self.discovered_readers if is_reader else self.discovered_writers is_new = endpoint_guid not in endpoint_map endpoint_map[endpoint_guid] = endpoint + if not is_reader: + subscribed_topics = {reader.topic_name for reader in self.local_readers} + if endpoint.topic_name in subscribed_topics: + for multicast_address, multicast_port in endpoint.multicast_locators: + if multicast_port == self.ports.user_multicast: + self._join_user_multicast_group(multicast_address) if is_new: kind = "reader" if is_reader else "writer" log( @@ -836,11 +906,14 @@ def handle_user_packet(self, packet: bytes, sender_ip: str, sender_port: int) -> subscribed_topics = {reader.topic_name for reader in self.local_readers} if topic_name in subscribed_topics: writer = self.local_writers[0] - self._publish_value(writer, maybe_value, (sender_ip, sender_port)) - log( - f"[echo] responded with value={maybe_value} on '{writer.topic_name}' " - f"to {sender_ip}:{sender_port}" - ) + if self._publish_value(writer, maybe_value): + log(f"[echo] responded with value={maybe_value} on '{writer.topic_name}'") + else: + self._publish_value(writer, maybe_value, (sender_ip, sender_port)) + log( + f"[echo] responded with value={maybe_value} on '{writer.topic_name}' " + f"to {sender_ip}:{sender_port}" + ) def run(self) -> None: start_time = time.monotonic() @@ -884,6 +957,7 @@ def run(self) -> None: self.metatraffic_multicast_sock, self.metatraffic_unicast_sock, self.user_unicast_sock, + self.user_multicast_sock, ], [], [], @@ -892,7 +966,7 @@ def run(self) -> None: for sock in readable: packet, sender = sock.recvfrom(4096) sender_ip, sender_port = sender[0], sender[1] - if sock is self.user_unicast_sock: + if sock is self.user_unicast_sock or sock is self.user_multicast_sock: self.handle_user_packet(packet, sender_ip, sender_port) else: self.handle_metatraffic_packet(packet, sender_ip) @@ -906,6 +980,7 @@ def close(self) -> None: self.metatraffic_multicast_sock, self.metatraffic_unicast_sock, self.user_unicast_sock, + self.user_multicast_sock, ): try: sock.close() From 2f9437603dfdaa093adb6f93329cbb275e704c2e Mon Sep 17 00:00:00 2001 From: William Emfinger Date: Tue, 16 Jun 2026 13:30:26 -0500 Subject: [PATCH 08/24] Potential fix for pull request finding 'Empty except' Co-authored-by: Copilot Autofix powered by AI <223894421+github-code-quality[bot]@users.noreply.github.com> --- python/rtps_host.py | 2 ++ 1 file changed, 2 insertions(+) diff --git a/python/rtps_host.py b/python/rtps_host.py index 0aabadb38..5cd6d4beb 100644 --- a/python/rtps_host.py +++ b/python/rtps_host.py @@ -545,6 +545,8 @@ def _create_user_multicast_socket(self) -> socket.socket: try: sock.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEPORT, 1) except OSError: + # Some platforms expose SO_REUSEPORT but reject setting it; this is + # a best-effort optimization and is not required for correctness. pass sock.bind((self.args.bind_address, self.ports.user_multicast)) sock.setblocking(False) From 3b529fb5a59fe2e814ad278c608ba21e19b85fb2 Mon Sep 17 00:00:00 2001 From: William Emfinger Date: Tue, 16 Jun 2026 14:08:39 -0500 Subject: [PATCH 09/24] Potential fix for pull request finding Co-authored-by: Copilot Autofix powered by AI <175728472+Copilot@users.noreply.github.com> --- components/rtps/src/rtps.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/components/rtps/src/rtps.cpp b/components/rtps/src/rtps.cpp index 83bf00d8d..0cdc1e7af 100644 --- a/components/rtps/src/rtps.cpp +++ b/components/rtps/src/rtps.cpp @@ -1065,8 +1065,9 @@ std::vector RtpsParticipant::build_uint32_data_message(const WriterConf payload_writer.append_bytes(cdr); auto guid = writer_guid(writer_config.entity_index); - return build_message(guid_prefix_, {.value = kEntityIdUnknown}, guid.entity_id, 1, - payload_writer.take()) + static std::atomic sequence_number{1}; + return build_message(guid_prefix_, {.value = kEntityIdUnknown}, guid.entity_id, + sequence_number.fetch_add(1), payload_writer.take()) .serialize(); } From c4d76ef5eaaac68c6c4ec9d1bb86b72841dbb62b Mon Sep 17 00:00:00 2001 From: William Emfinger Date: Tue, 16 Jun 2026 14:09:07 -0500 Subject: [PATCH 10/24] Potential fix for pull request finding Co-authored-by: Copilot Autofix powered by AI <175728472+Copilot@users.noreply.github.com> --- components/rtps/src/rtps.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/components/rtps/src/rtps.cpp b/components/rtps/src/rtps.cpp index 0cdc1e7af..7b92f65f1 100644 --- a/components/rtps/src/rtps.cpp +++ b/components/rtps/src/rtps.cpp @@ -982,8 +982,9 @@ std::vector RtpsParticipant::build_spdp_announce_message() const { append_parameter_sentinel(parameters); auto payload = build_parameter_list_payload(parameters); - return build_message(guid_prefix_, {.value = kEntityIdUnknown}, {.value = kSpdpWriterEntityId}, 1, - payload) + static std::atomic sequence_number{1}; + return build_message(guid_prefix_, {.value = kEntityIdUnknown}, {.value = kSpdpWriterEntityId}, + sequence_number.fetch_add(1), payload) .serialize(); } From 78d6a3e9a09ce4882177cecc5fa26ad428df4684 Mon Sep 17 00:00:00 2001 From: William Emfinger Date: Tue, 16 Jun 2026 14:09:39 -0500 Subject: [PATCH 11/24] Potential fix for pull request finding Co-authored-by: Copilot Autofix powered by AI <175728472+Copilot@users.noreply.github.com> --- components/rtps/src/rtps.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/components/rtps/src/rtps.cpp b/components/rtps/src/rtps.cpp index 7b92f65f1..ebf0c6683 100644 --- a/components/rtps/src/rtps.cpp +++ b/components/rtps/src/rtps.cpp @@ -1015,9 +1015,10 @@ RtpsParticipant::build_sedp_publication_message(const WriterConfig &writer_confi append_parameter_sentinel(parameters); auto payload = build_parameter_list_payload(parameters); + static std::atomic sequence_number{1}; return build_message(guid_prefix_, {.value = kSedpPublicationsReaderEntityId}, {.value = kSedpPublicationsWriterEntityId}, - static_cast(writer_config.entity_index + 1), payload) + sequence_number.fetch_add(1), payload) .serialize(); } From 81f7bd69791d156e581dec7b85af70a95fde960d Mon Sep 17 00:00:00 2001 From: William Emfinger Date: Tue, 16 Jun 2026 14:10:00 -0500 Subject: [PATCH 12/24] Potential fix for pull request finding Co-authored-by: Copilot Autofix powered by AI <175728472+Copilot@users.noreply.github.com> --- components/rtps/src/rtps.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/components/rtps/src/rtps.cpp b/components/rtps/src/rtps.cpp index ebf0c6683..adc9bdd08 100644 --- a/components/rtps/src/rtps.cpp +++ b/components/rtps/src/rtps.cpp @@ -1048,9 +1048,10 @@ RtpsParticipant::build_sedp_subscription_message(const ReaderConfig &reader_conf append_parameter_sentinel(parameters); auto payload = build_parameter_list_payload(parameters); + static std::atomic sequence_number{1}; return build_message(guid_prefix_, {.value = kSedpSubscriptionsReaderEntityId}, {.value = kSedpSubscriptionsWriterEntityId}, - static_cast(reader_config.entity_index + 1), payload) + sequence_number.fetch_add(1), payload) .serialize(); } From 70da4b655915d18f4b365c0789186fb926e8517c Mon Sep 17 00:00:00 2001 From: William Emfinger Date: Tue, 16 Jun 2026 14:11:01 -0500 Subject: [PATCH 13/24] Potential fix for pull request finding Co-authored-by: Copilot Autofix powered by AI <175728472+Copilot@users.noreply.github.com> --- components/rtps/example/main/Kconfig.projbuild | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/components/rtps/example/main/Kconfig.projbuild b/components/rtps/example/main/Kconfig.projbuild index cdeb799f7..7d4a64dbc 100644 --- a/components/rtps/example/main/Kconfig.projbuild +++ b/components/rtps/example/main/Kconfig.projbuild @@ -30,7 +30,7 @@ menu "RTPS Example Configuration" config RTPS_EXAMPLE_DOMAIN_ID int "RTPS domain ID" - range 0 232 + range 0 231 default 0 help Both boards must use the same domain ID to discover each other. From ac317c6f17f5a3d3162be2c46eba328b86868a67 Mon Sep 17 00:00:00 2001 From: William Emfinger Date: Tue, 16 Jun 2026 14:11:41 -0500 Subject: [PATCH 14/24] Potential fix for pull request finding Co-authored-by: Copilot Autofix powered by AI <175728472+Copilot@users.noreply.github.com> --- components/rtps/src/rtps.cpp | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/components/rtps/src/rtps.cpp b/components/rtps/src/rtps.cpp index adc9bdd08..79f3379d8 100644 --- a/components/rtps/src/rtps.cpp +++ b/components/rtps/src/rtps.cpp @@ -1213,7 +1213,10 @@ bool RtpsParticipant::handle_metatraffic_message(std::vector &data, find_parameter(parameters, ParameterId::PID_DEFAULT_UNICAST_LOCATOR)) { if (auto maybe_locator = parse_locator(maybe_default_unicast_parameter->value)) { participant.ports.user_unicast = static_cast(maybe_locator->port); - participant.address = maybe_locator->address_string(); + const auto advertised_address = maybe_locator->address_string(); + if (advertised_address != "0.0.0.0") { + participant.address = advertised_address; + } } } if (auto maybe_default_multicast_parameter = From 20f4cd3b0802e20469ada50906561e87eccfd6c2 Mon Sep 17 00:00:00 2001 From: William Emfinger Date: Tue, 16 Jun 2026 14:20:38 -0500 Subject: [PATCH 15/24] actually fix issues --- components/rtps/include/rtps.hpp | 10 +++++++ components/rtps/src/rtps.cpp | 45 ++++++++++++++++++++++---------- 2 files changed, 41 insertions(+), 14 deletions(-) diff --git a/components/rtps/include/rtps.hpp b/components/rtps/include/rtps.hpp index e861cfab8..abe05a5aa 100644 --- a/components/rtps/include/rtps.hpp +++ b/components/rtps/include/rtps.hpp @@ -11,6 +11,7 @@ #include #include #include +#include #include #include "base_component.hpp" @@ -367,6 +368,10 @@ class RtpsParticipant : public BaseComponent { bool ensure_user_multicast_receivers_started(); std::vector build_user_send_configs(std::string_view topic_name, const WriterConfig &writer_config) const; + int64_t next_spdp_sequence_number() const; + int64_t next_sedp_publication_sequence_number() const; + int64_t next_sedp_subscription_sequence_number() const; + int64_t next_user_data_sequence_number(uint32_t entity_index) const; bool send_spdp_announce_now(); bool send_sedp_announcements_to(const ParticipantProxy &participant); bool send_discovery_now(); @@ -383,6 +388,11 @@ class RtpsParticipant : public BaseComponent { std::unique_ptr announce_task_; mutable std::mutex mutex_; + mutable std::mutex sequence_mutex_; + mutable std::atomic spdp_sequence_number_{1}; + mutable std::atomic sedp_publications_sequence_number_{1}; + mutable std::atomic sedp_subscriptions_sequence_number_{1}; + mutable std::unordered_map user_data_sequence_numbers_; std::vector writers_; std::vector readers_; std::vector discovered_participants_; diff --git a/components/rtps/src/rtps.cpp b/components/rtps/src/rtps.cpp index 79f3379d8..1a96e4b7a 100644 --- a/components/rtps/src/rtps.cpp +++ b/components/rtps/src/rtps.cpp @@ -477,11 +477,8 @@ std::optional find_parameter(std::span param std::vector find_parameters(std::span parameters, ParameterId id) { std::vector matches; - for (const auto ¶meter : parameters) { - if (parameter.id == id) { - matches.push_back(parameter); - } - } + std::copy_if(parameters.begin(), parameters.end(), std::back_inserter(matches), + [id](const auto ¶meter) { return parameter.id == id; }); return matches; } @@ -562,7 +559,9 @@ std::optional parse_locator(std::span @@ -982,9 +981,8 @@ std::vector RtpsParticipant::build_spdp_announce_message() const { append_parameter_sentinel(parameters); auto payload = build_parameter_list_payload(parameters); - static std::atomic sequence_number{1}; return build_message(guid_prefix_, {.value = kEntityIdUnknown}, {.value = kSpdpWriterEntityId}, - sequence_number.fetch_add(1), payload) + next_spdp_sequence_number(), payload) .serialize(); } @@ -1015,10 +1013,9 @@ RtpsParticipant::build_sedp_publication_message(const WriterConfig &writer_confi append_parameter_sentinel(parameters); auto payload = build_parameter_list_payload(parameters); - static std::atomic sequence_number{1}; return build_message(guid_prefix_, {.value = kSedpPublicationsReaderEntityId}, {.value = kSedpPublicationsWriterEntityId}, - sequence_number.fetch_add(1), payload) + next_sedp_publication_sequence_number(), payload) .serialize(); } @@ -1048,10 +1045,9 @@ RtpsParticipant::build_sedp_subscription_message(const ReaderConfig &reader_conf append_parameter_sentinel(parameters); auto payload = build_parameter_list_payload(parameters); - static std::atomic sequence_number{1}; return build_message(guid_prefix_, {.value = kSedpSubscriptionsReaderEntityId}, {.value = kSedpSubscriptionsWriterEntityId}, - sequence_number.fetch_add(1), payload) + next_sedp_subscription_sequence_number(), payload) .serialize(); } @@ -1068,9 +1064,9 @@ std::vector RtpsParticipant::build_uint32_data_message(const WriterConf payload_writer.append_bytes(cdr); auto guid = writer_guid(writer_config.entity_index); - static std::atomic sequence_number{1}; return build_message(guid_prefix_, {.value = kEntityIdUnknown}, guid.entity_id, - sequence_number.fetch_add(1), payload_writer.take()) + next_user_data_sequence_number(writer_config.entity_index), + payload_writer.take()) .serialize(); } @@ -1114,6 +1110,27 @@ bool RtpsParticipant::publish_uint32(std::string_view topic_name, uint32_t value return sent; } +int64_t RtpsParticipant::next_spdp_sequence_number() const { + return spdp_sequence_number_.fetch_add(1, std::memory_order_relaxed); +} + +int64_t RtpsParticipant::next_sedp_publication_sequence_number() const { + return sedp_publications_sequence_number_.fetch_add(1, std::memory_order_relaxed); +} + +int64_t RtpsParticipant::next_sedp_subscription_sequence_number() const { + return sedp_subscriptions_sequence_number_.fetch_add(1, std::memory_order_relaxed); +} + +int64_t RtpsParticipant::next_user_data_sequence_number(uint32_t entity_index) const { + std::lock_guard lock(sequence_mutex_); + auto iterator = user_data_sequence_numbers_.try_emplace(entity_index, 1).first; + auto &sequence_number = iterator->second; + int64_t current = sequence_number; + sequence_number++; + return current; +} + std::vector RtpsParticipant::serialize_uint32_cdr(uint32_t value) { espp::CdrWriter writer({ .encapsulation = espp::CdrEncapsulation::CDR_LE, From 1390dbe71e4b086e73fd35f2dc09f803fd73e6c9 Mon Sep 17 00:00:00 2001 From: William Emfinger Date: Tue, 16 Jun 2026 17:07:05 -0500 Subject: [PATCH 16/24] fix issues --- components/cdr/include/cdr.hpp | 15 ++++- components/rtps/include/rtps.hpp | 9 ++- components/rtps/src/rtps.cpp | 112 ++++++++++++++++++++++++------- 3 files changed, 106 insertions(+), 30 deletions(-) diff --git a/components/cdr/include/cdr.hpp b/components/cdr/include/cdr.hpp index c8f47a216..f595230fd 100644 --- a/components/cdr/include/cdr.hpp +++ b/components/cdr/include/cdr.hpp @@ -75,6 +75,11 @@ class CdrWriter { /// @brief Create a writer configured for a headerless/body-only CDR payload. /// @param encapsulation Endianness/encapsulation rules to use for the body payload. /// @return A ready-to-use writer with no encapsulation header in its output. + /// @note CDR alignment is measured from the start of the buffer. Because a body-only writer has + /// no 4-byte encapsulation header, 8-byte-aligned members (e.g. int64/double) land at different + /// offsets than in an encapsulated writer. A body produced here and later wrapped with + /// encapsulate() is therefore not byte-compatible with a directly-encapsulated buffer when it + /// contains 8-byte-aligned types. Current RTPS usage only emits <= 4-byte-aligned types. [[nodiscard]] static CdrWriter make_body_writer(CdrEncapsulation encapsulation = CdrEncapsulation::CDR_LE) { return CdrWriter(body_config(encapsulation)); @@ -424,6 +429,12 @@ class CdrReader { span = span.first(span.size() - 1); } text.assign(reinterpret_cast(span.data()), span.size()); + // Re-align for any following element. CDR only inserts padding before an aligned element, so a + // trailing string at the end of the buffer may legitimately have no padding bytes; only align + // when there are bytes left to consume so a valid final string is not rejected. + if (remaining() == 0) { + return true; + } return align(4); } @@ -500,7 +511,9 @@ class CdrReader { return false; } values.clear(); - values.reserve(length); + // Cap the reservation against the bytes actually available so a malformed length cannot trigger + // a huge allocation. The element loop below still validates each read. + values.reserve(std::min(length, remaining() / sizeof(T))); for (uint32_t i = 0; i < length; i++) { T value{}; if (!read(value)) { diff --git a/components/rtps/include/rtps.hpp b/components/rtps/include/rtps.hpp index abe05a5aa..865c8a395 100644 --- a/components/rtps/include/rtps.hpp +++ b/components/rtps/include/rtps.hpp @@ -283,12 +283,12 @@ class RtpsParticipant : public BaseComponent { std::vector discovered_readers() const; /// @brief Access the registered local writer configurations. - /// @return A const reference to the local writer list. - const std::vector &writers() const; + /// @return A snapshot copy of the local writer list. + std::vector writers() const; /// @brief Access the registered local reader configurations. - /// @return A const reference to the local reader list. - const std::vector &readers() const; + /// @return A snapshot copy of the local reader list. + std::vector readers() const; /// @brief Compute the standard RTPS UDP port mapping for this participant. /// @return The derived metatraffic and user-data ports for the configured domain and participant @@ -375,7 +375,6 @@ class RtpsParticipant : public BaseComponent { bool send_spdp_announce_now(); bool send_sedp_announcements_to(const ParticipantProxy &participant); bool send_discovery_now(); - ParticipantProxy make_local_participant_proxy() const; Config config_; GuidPrefix guid_prefix_{}; diff --git a/components/rtps/src/rtps.cpp b/components/rtps/src/rtps.cpp index 1a96e4b7a..b1e6ad7a6 100644 --- a/components/rtps/src/rtps.cpp +++ b/components/rtps/src/rtps.cpp @@ -176,6 +176,20 @@ class ByteReader { return true; } + bool read_u16_be(uint16_t &value) { + if (remaining() < 2) { + return false; + } + value = static_cast(static_cast(data_[offset_]) << 8) | + static_cast(data_[offset_ + 1]); + offset_ += 2; + return true; + } + + bool read_u16(uint16_t &value, bool little_endian) { + return little_endian ? read_u16_le(value) : read_u16_be(value); + } + bool read_u32_le(uint32_t &value) { if (remaining() < 4) { return false; @@ -209,13 +223,27 @@ class ByteReader { return true; } - bool read_sequence_number_le(int64_t &value) { - int32_t high = 0; + bool read_sequence_number(int64_t &value, bool little_endian) { + uint32_t high = 0; uint32_t low = 0; - if (!read_i32_le(high) || !read_u32_le(low)) { + if (little_endian) { + if (!read_u32_le(high) || !read_u32_le(low)) { + return false; + } + } else { + if (!read_u32_be(high) || !read_u32_be(low)) { + return false; + } + } + value = (static_cast(static_cast(high)) << 32) | low; + return true; + } + + bool skip(size_t length) { + if (remaining() < length) { return false; } - value = (static_cast(high) << 32) | low; + offset_ += length; return true; } @@ -378,22 +406,27 @@ void append_parameter_locator(ByteWriter &writer, ParameterId id, } void append_parameter_string_cdr(ByteWriter &writer, ParameterId id, std::string_view text) { - uint16_t raw_length = static_cast(4 + text.size() + 1); - append_parameter_header(writer, id, raw_length); auto cdr_writer = espp::CdrWriter::make_body_writer(espp::CdrEncapsulation::CDR_LE); cdr_writer.write_string(text); - writer.append_bytes(cdr_writer.payload()); + auto cdr_payload = cdr_writer.payload(); + // The RTPS PL_CDR encoding requires parameterLength to be a multiple of 4 so the next + // parameter starts 4-byte aligned. write_string() already trailing-aligns the body to 4, so the + // payload length is the padded length we must declare. + append_parameter_header(writer, id, static_cast(cdr_payload.size())); + writer.append_bytes(cdr_payload); } void append_parameter_octet_sequence(ByteWriter &writer, ParameterId id, std::span bytes) { - uint16_t raw_length = static_cast(4 + bytes.size()); - append_parameter_header(writer, id, raw_length); auto cdr_writer = espp::CdrWriter::make_body_writer(espp::CdrEncapsulation::CDR_LE); cdr_writer.write(static_cast(bytes.size())); cdr_writer.write_bytes(bytes); cdr_writer.align(4); - writer.append_bytes(cdr_writer.payload()); + auto cdr_payload = cdr_writer.payload(); + // parameterLength must be a multiple of 4 (see append_parameter_string_cdr); the align(4) above + // padded the payload to that length, so declare the padded length. + append_parameter_header(writer, id, static_cast(cdr_payload.size())); + writer.append_bytes(cdr_payload); } void append_parameter_reliability(ByteWriter &writer, @@ -437,6 +470,11 @@ void append_parameter_sentinel(ByteWriter &writer) { std::vector parse_parameter_list(std::span payload) { std::vector parameters; espp::CdrReader cdr_reader(payload); + // Limitation: only little-endian parameter lists (PL_CDR_LE) are decoded. The parameter value + // parsers below (parse_u32_le, parse_locator, parse_guid, ...) assume little-endian contents, so + // a big-endian (PL_CDR_BE) list is intentionally rejected rather than misparsed. In practice DDS + // and ROS 2 implementations emit PL_CDR_LE for SPDP/SEDP discovery, so this is a discovery-only + // gap. if (!cdr_reader.valid() || cdr_reader.encapsulation() != espp::CdrEncapsulation::PL_CDR_LE) { return parameters; } @@ -600,6 +638,25 @@ bool is_same_guid_prefix(const espp::RtpsParticipant::Guid &guid, return guid.prefix == prefix; } +// Skip an inline-QoS parameter list (a raw ParameterList without an encapsulation header) up to and +// including its PID_SENTINEL terminator. Returns false if the list is malformed/truncated. +bool skip_inline_qos(ByteReader &reader, bool little_endian) { + while (reader.remaining() >= 4) { + uint16_t pid = 0; + uint16_t length = 0; + if (!reader.read_u16(pid, little_endian) || !reader.read_u16(length, little_endian)) { + return false; + } + if (pid == static_cast(ParameterId::PID_SENTINEL)) { + return true; + } + if (!reader.skip(length)) { + return false; + } + } + return false; +} + DataSubmessageView parse_data_submessage(const espp::RtpsParticipant::Submessage &submessage, bool &ok) { DataSubmessageView view; @@ -609,21 +666,35 @@ DataSubmessageView parse_data_submessage(const espp::RtpsParticipant::Submessage return view; } + const bool little_endian = (submessage.flags & kSubmessageFlagLittleEndian) != 0; ByteReader reader(std::span{submessage.payload.data(), submessage.payload.size()}); uint16_t extra_flags = 0; uint16_t octets_to_inline_qos = 0; - if (!reader.read_u16_le(extra_flags) || !reader.read_u16_le(octets_to_inline_qos) || + if (!reader.read_u16(extra_flags, little_endian) || + !reader.read_u16(octets_to_inline_qos, little_endian) || !reader.read_bytes( std::span{view.reader_id.value.data(), view.reader_id.value.size()}) || !reader.read_bytes( std::span{view.writer_id.value.data(), view.writer_id.value.size()}) || - !reader.read_sequence_number_le(view.writer_sn)) { + !reader.read_sequence_number(view.writer_sn, little_endian)) { return view; } view.inline_qos_present = (submessage.flags & kSubmessageFlagInlineQos) != 0; view.data_present = true; - if (view.inline_qos_present || octets_to_inline_qos != kDataSubmessageOctetsToInlineQos) { + + // octetsToInlineQos counts from the byte after the octetsToInlineQos field to the start of the + // inline QoS (or the serialized payload when no inline QoS is present). We have already consumed + // the standard 16-byte readerId+writerId+writerSN block; honor any additional header octets a + // sender may have included instead of assuming the fixed layout. + if (octets_to_inline_qos < kDataSubmessageOctetsToInlineQos || + !reader.skip(octets_to_inline_qos - kDataSubmessageOctetsToInlineQos)) { + return view; + } + + // When inline QoS is present, skip past the inline QoS parameter list to reach the serialized + // payload rather than dropping the sample. + if (view.inline_qos_present && !skip_inline_qos(reader, little_endian)) { return view; } @@ -919,11 +990,13 @@ std::vector RtpsParticipant::discovered_readers( return discovered_readers_; } -const std::vector &RtpsParticipant::writers() const { +std::vector RtpsParticipant::writers() const { + std::lock_guard lock(mutex_); return writers_; } -const std::vector &RtpsParticipant::readers() const { +std::vector RtpsParticipant::readers() const { + std::lock_guard lock(mutex_); return readers_; } @@ -1616,13 +1689,4 @@ bool RtpsParticipant::send_discovery_now() { }); } -RtpsParticipant::ParticipantProxy RtpsParticipant::make_local_participant_proxy() const { - return {.participant_guid = participant_guid(), - .guid_prefix = guid_prefix_, - .name = config_.node_name, - .enclave = config_.enclave, - .address = config_.advertised_address, - .ports = ports(), - .builtin_endpoints = kBuiltinEndpointSet}; -} } // namespace espp From aa4cfd210e8070374b82422bca84c29509ba11a7 Mon Sep 17 00:00:00 2001 From: William Emfinger Date: Tue, 16 Jun 2026 20:09:53 -0500 Subject: [PATCH 17/24] separate socket and rtps log levels --- components/rtps/example/main/rtps_example.cpp | 3 +++ components/rtps/include/rtps.hpp | 5 +++++ components/rtps/src/rtps.cpp | 9 +++++---- 3 files changed, 13 insertions(+), 4 deletions(-) diff --git a/components/rtps/example/main/rtps_example.cpp b/components/rtps/example/main/rtps_example.cpp index 6cea5bdc4..3abe97f85 100644 --- a/components/rtps/example/main/rtps_example.cpp +++ b/components/rtps/example/main/rtps_example.cpp @@ -132,6 +132,9 @@ extern "C" void app_main(void) { endpoint.topic_name, endpoint.type_name); }, .log_level = espp::Logger::Verbosity::INFO, + // Keep the underlying UDP sockets quieter than the participant so routine socket activity + // does not clutter the logs. Raise this to debug transport issues independently. + .socket_log_level = espp::Logger::Verbosity::WARN, }); #if CONFIG_RTPS_EXAMPLE_ROLE_INITIATOR diff --git a/components/rtps/include/rtps.hpp b/components/rtps/include/rtps.hpp index 865c8a395..57356ebe9 100644 --- a/components/rtps/include/rtps.hpp +++ b/components/rtps/include/rtps.hpp @@ -238,6 +238,11 @@ class RtpsParticipant : public BaseComponent { nullptr}; ///< Callback invoked when a remote endpoint is first discovered. espp::Logger::Verbosity log_level{ espp::Logger::Verbosity::INFO}; ///< Participant log verbosity. + espp::Logger::Verbosity socket_log_level{ + espp::Logger::Verbosity::WARN}; ///< Log verbosity for the participant's underlying UDP + ///< sockets. Defaults to WARN so routine socket activity + ///< does not clutter the logs; raise it to debug transport + ///< issues independently of the participant log level. }; /// @brief Construct an RTPS participant. diff --git a/components/rtps/src/rtps.cpp b/components/rtps/src/rtps.cpp index b1e6ad7a6..07be446e5 100644 --- a/components/rtps/src/rtps.cpp +++ b/components/rtps/src/rtps.cpp @@ -845,11 +845,11 @@ bool RtpsParticipant::start() { auto port_mapping = ports(); metatraffic_multicast_receiver_ = - std::make_unique(UdpSocket::Config{.log_level = get_log_level()}); + std::make_unique(UdpSocket::Config{.log_level = config_.socket_log_level}); metatraffic_unicast_receiver_ = - std::make_unique(UdpSocket::Config{.log_level = get_log_level()}); + std::make_unique(UdpSocket::Config{.log_level = config_.socket_log_level}); user_unicast_receiver_ = - std::make_unique(UdpSocket::Config{.log_level = get_log_level()}); + std::make_unique(UdpSocket::Config{.log_level = config_.socket_log_level}); auto multicast_task_config = config_.receive_task_config; multicast_task_config.name = config_.receive_task_config.name + "_spdp_mc"; @@ -1532,7 +1532,8 @@ bool RtpsParticipant::ensure_user_multicast_receivers_started() { continue; } - auto socket = std::make_unique(UdpSocket::Config{.log_level = get_log_level()}); + auto socket = + std::make_unique(UdpSocket::Config{.log_level = config_.socket_log_level}); auto task_config = config_.receive_task_config; task_config.name = fmt::format("{}_user_mc_{}", config_.receive_task_config.name, user_multicast_receivers_.size()); From 9e16e288a7bf752bc36288cbe31acf0121b2dadd Mon Sep 17 00:00:00 2001 From: William Emfinger Date: Wed, 17 Jun 2026 13:10:47 -0500 Subject: [PATCH 18/24] improve robustness and api correctness, esp. w.r.t. string operations --- components/cdr/include/cdr.hpp | 20 +++++++++++++----- components/rtps/include/rtps.hpp | 2 +- components/rtps/src/rtps.cpp | 35 ++++++++++++++++++++------------ 3 files changed, 38 insertions(+), 19 deletions(-) diff --git a/components/cdr/include/cdr.hpp b/components/cdr/include/cdr.hpp index f595230fd..1dff9d53d 100644 --- a/components/cdr/include/cdr.hpp +++ b/components/cdr/include/cdr.hpp @@ -425,9 +425,13 @@ class CdrReader { } auto span = data_.subspan(offset_, length); offset_ += length; - if (span.back() == 0) { - span = span.first(span.size() - 1); + // CDR strings are length-prefixed and null-terminated; a missing terminator is a malformed + // payload, so reject it rather than silently accepting the bytes. + if (span.back() != 0) { + valid_ = false; + return false; } + span = span.first(span.size() - 1); text.assign(reinterpret_cast(span.data()), span.size()); // Re-align for any following element. CDR only inserts padding before an aligned element, so a // trailing string at the end of the buffer may legitimately have no padding bytes; only align @@ -510,10 +514,16 @@ class CdrReader { if (!read(length)) { return false; } + // Bound the declared length against the bytes actually available before reserving so a + // malformed/malicious payload cannot request an enormous allocation (or OOM) on a + // memory-constrained target. Each element occupies at least sizeof(T) bytes, so a length larger + // than remaining() / sizeof(T) cannot possibly be satisfied; reject it up front. + if (length > remaining() / sizeof(T)) { + valid_ = false; + return false; + } values.clear(); - // Cap the reservation against the bytes actually available so a malformed length cannot trigger - // a huge allocation. The element loop below still validates each read. - values.reserve(std::min(length, remaining() / sizeof(T))); + values.reserve(length); for (uint32_t i = 0; i < length; i++) { T value{}; if (!read(value)) { diff --git a/components/rtps/include/rtps.hpp b/components/rtps/include/rtps.hpp index 57356ebe9..841e6d8a3 100644 --- a/components/rtps/include/rtps.hpp +++ b/components/rtps/include/rtps.hpp @@ -370,7 +370,7 @@ class RtpsParticipant : public BaseComponent { bool handle_metatraffic_message(std::vector &data, const Socket::Info &sender); bool handle_user_message(std::vector &data, const Socket::Info &sender); - bool ensure_user_multicast_receivers_started(); + bool ensure_user_multicast_receivers_started(const std::string &extra_group = {}); std::vector build_user_send_configs(std::string_view topic_name, const WriterConfig &writer_config) const; int64_t next_spdp_sequence_number() const; diff --git a/components/rtps/src/rtps.cpp b/components/rtps/src/rtps.cpp index 07be446e5..336bb063d 100644 --- a/components/rtps/src/rtps.cpp +++ b/components/rtps/src/rtps.cpp @@ -626,6 +626,10 @@ std::string extract_enclave(std::span user_data_bytes) { if (end == std::string::npos) { end = text.size(); } + // Normalize an empty enclave (e.g. "enclave=;") to the default "/" rather than returning "". + if (end == position) { + return "/"; + } return text.substr(position, end - position); } @@ -963,15 +967,15 @@ bool RtpsParticipant::add_writer(const WriterConfig &writer_config) { } bool RtpsParticipant::add_reader(const ReaderConfig &reader_config) { - { - std::lock_guard lock(mutex_); - readers_.push_back(reader_config); - } + // Bring up the reader's multicast receiver (if any) before persisting the reader, so a failure + // does not leave the participant with a registered reader that has no working receiver. if (started_.load() && !reader_config.multicast_group.empty() && - !ensure_user_multicast_receivers_started()) { + !ensure_user_multicast_receivers_started(reader_config.multicast_group)) { logger_.error("Failed to start multicast receiver for topic '{}'", reader_config.topic_name); return false; } + std::lock_guard lock(mutex_); + readers_.push_back(reader_config); return true; } @@ -1503,23 +1507,28 @@ bool RtpsParticipant::handle_user_message(std::vector &data, const Sock return false; } -bool RtpsParticipant::ensure_user_multicast_receivers_started() { +bool RtpsParticipant::ensure_user_multicast_receivers_started(const std::string &extra_group) { if (!started_.load()) { return true; } std::vector desired_groups; - if (config_.use_multicast_for_user_data && !config_.user_multicast_group.empty()) { - desired_groups.push_back(config_.user_multicast_group); + auto add_group = [&desired_groups](const std::string &group) { + if (!group.empty() && + std::find(desired_groups.begin(), desired_groups.end(), group) == desired_groups.end()) { + desired_groups.push_back(group); + } + }; + if (config_.use_multicast_for_user_data) { + add_group(config_.user_multicast_group); } + // Include the group of a reader being added before it is persisted in readers_, so the receiver + // can be brought up (and any failure surfaced) without leaving the reader half-registered. + add_group(extra_group); { std::lock_guard lock(mutex_); for (const auto &reader_config : readers_) { - if (!reader_config.multicast_group.empty() && - std::find(desired_groups.begin(), desired_groups.end(), reader_config.multicast_group) == - desired_groups.end()) { - desired_groups.push_back(reader_config.multicast_group); - } + add_group(reader_config.multicast_group); } } From 1e23f185b4429168bfe3ef8773e99977114ce1a6 Mon Sep 17 00:00:00 2001 From: William Emfinger Date: Wed, 17 Jun 2026 13:26:58 -0500 Subject: [PATCH 19/24] fix race --- components/rtps/include/rtps.hpp | 2 ++ components/rtps/src/rtps.cpp | 18 ++++++++++++++---- 2 files changed, 16 insertions(+), 4 deletions(-) diff --git a/components/rtps/include/rtps.hpp b/components/rtps/include/rtps.hpp index 841e6d8a3..45500ef70 100644 --- a/components/rtps/include/rtps.hpp +++ b/components/rtps/include/rtps.hpp @@ -392,6 +392,8 @@ class RtpsParticipant : public BaseComponent { std::unique_ptr announce_task_; mutable std::mutex mutex_; + mutable std::mutex receivers_mutex_; ///< Guards user_multicast_receivers_ against concurrent + ///< add_reader()/stop() access. mutable std::mutex sequence_mutex_; mutable std::atomic spdp_sequence_number_{1}; mutable std::atomic sedp_publications_sequence_number_{1}; diff --git a/components/rtps/src/rtps.cpp b/components/rtps/src/rtps.cpp index 336bb063d..d8e6f88db 100644 --- a/components/rtps/src/rtps.cpp +++ b/components/rtps/src/rtps.cpp @@ -813,6 +813,10 @@ RtpsParticipant::Message::parse(std::span data) { Submessage submessage; uint8_t kind = 0; uint16_t length = 0; + // Limitation: submessageLength is read little-endian regardless of the submessage E-flag (bit 0 + // of flags). Big-endian submessages are not supported; in practice DDS/ROS 2 peers emit + // little-endian framing. Endianness of the DATA submessage body itself is honored separately in + // parse_data_submessage(). if (!reader.read_u8(kind) || !reader.read_u8(submessage.flags) || !reader.read_u16_le(length)) { return std::nullopt; } @@ -946,12 +950,15 @@ void RtpsParticipant::stop() { metatraffic_unicast_receiver_->stop_receiving(); metatraffic_unicast_receiver_.reset(); } - for (auto &receiver : user_multicast_receivers_) { - if (receiver.socket) { - receiver.socket->stop_receiving(); + { + std::lock_guard receivers_lock(receivers_mutex_); + for (auto &receiver : user_multicast_receivers_) { + if (receiver.socket) { + receiver.socket->stop_receiving(); + } } + user_multicast_receivers_.clear(); } - user_multicast_receivers_.clear(); if (user_unicast_receiver_) { user_unicast_receiver_->stop_receiving(); user_unicast_receiver_.reset(); @@ -1533,6 +1540,9 @@ bool RtpsParticipant::ensure_user_multicast_receivers_started(const std::string } auto port_mapping = ports(); + // receivers_mutex_ (not mutex_) guards user_multicast_receivers_; desired_groups was built above + // under mutex_, which has already been released, so the two locks are never held nested. + std::lock_guard receivers_lock(receivers_mutex_); for (const auto &group : desired_groups) { auto existing = std::find_if(user_multicast_receivers_.begin(), user_multicast_receivers_.end(), From 3943cb41ebdbb55d0755e0e3e350b9013b134187 Mon Sep 17 00:00:00 2001 From: William Emfinger Date: Wed, 17 Jun 2026 16:57:48 -0500 Subject: [PATCH 20/24] improve spec compliance --- components/rtps/src/rtps.cpp | 38 ++++++++++++++++++++---------------- 1 file changed, 21 insertions(+), 17 deletions(-) diff --git a/components/rtps/src/rtps.cpp b/components/rtps/src/rtps.cpp index d8e6f88db..319a348f6 100644 --- a/components/rtps/src/rtps.cpp +++ b/components/rtps/src/rtps.cpp @@ -62,6 +62,9 @@ constexpr int32_t kDefaultLeaseDurationSeconds = 20; constexpr uint32_t kDefaultLeaseDurationNanoseconds = 0; constexpr int32_t kDefaultMaxBlockingSeconds = 0; constexpr uint32_t kDefaultMaxBlockingNanoseconds = 100000000; +// PID_TYPE_MAX_SIZE_SERIALIZED carries the max CDR-serialized size of the type *including* the +// 4-byte encapsulation header (matching FastDDS: getMaxCdrSerializedSize() + 4). A UInt32 body is +// 4 bytes, so the spec-exact advertised value is 4 + 4 = 8. constexpr uint32_t kUInt32SerializedSize = 8; enum class ParameterId : uint16_t { @@ -113,11 +116,6 @@ class ByteWriter { data_.push_back(static_cast((value >> 8) & 0xff)); } - void append_u16_be(uint16_t value) { - data_.push_back(static_cast((value >> 8) & 0xff)); - data_.push_back(static_cast(value & 0xff)); - } - void append_u32_le(uint32_t value) { for (int i = 0; i < 4; i++) { data_.push_back(static_cast((value >> (8 * i)) & 0xff)); @@ -126,12 +124,6 @@ class ByteWriter { void append_i32_le(int32_t value) { append_u32_le(static_cast(value)); } - void append_u32_be(uint32_t value) { - for (int i = 3; i >= 0; i--) { - data_.push_back(static_cast((value >> (8 * i)) & 0xff)); - } - } - void append_sequence_number_le(int64_t value) { auto high = static_cast(value >> 32); auto low = static_cast(value & 0xffffffffu); @@ -390,18 +382,28 @@ void append_parameter_bool(ByteWriter &writer, ParameterId id, bool value) { writer.append_u8(0); } +// RTPS Duration_t/Time_t use the NTP representation {int32 seconds, uint32 fraction} where the +// fraction is in units of 1/2^32 of a second (see DDSI-RTPS; OpenDDS RtpsCore.idl references RFC +// 1305). Convert a nanosecond count to that fraction so durations are encoded spec-exactly. +constexpr uint32_t ntp_fraction_from_nanoseconds(uint32_t nanoseconds) { + return static_cast((static_cast(nanoseconds) << 32) / 1000000000ULL); +} + void append_parameter_duration(ByteWriter &writer, ParameterId id, int32_t seconds, uint32_t nanoseconds) { append_parameter_header(writer, id, 8); writer.append_i32_le(seconds); - writer.append_u32_le(nanoseconds); + writer.append_u32_le(ntp_fraction_from_nanoseconds(nanoseconds)); } void append_parameter_locator(ByteWriter &writer, ParameterId id, const espp::RtpsParticipant::Locator &locator) { append_parameter_header(writer, id, 24); - writer.append_u32_be(static_cast(locator.kind)); - writer.append_u32_be(locator.port); + // Locator_t.kind and .port are CDR long/unsigned long encoded in the parameter list endianness + // (little-endian for PL_CDR_LE); only the 16-byte address is a raw per-byte (network-order) + // field. + writer.append_u32_le(static_cast(locator.kind)); + writer.append_u32_le(locator.port); writer.append_bytes(locator.address); } @@ -436,7 +438,7 @@ void append_parameter_reliability(ByteWriter &writer, ? kReliabilityReliable : kReliabilityBestEffort); writer.append_i32_le(kDefaultMaxBlockingSeconds); - writer.append_u32_le(kDefaultMaxBlockingNanoseconds); + writer.append_u32_le(ntp_fraction_from_nanoseconds(kDefaultMaxBlockingNanoseconds)); } void append_parameter_durability(ByteWriter &writer) { @@ -448,7 +450,7 @@ void append_parameter_liveliness(ByteWriter &writer) { append_parameter_header(writer, ParameterId::PID_LIVELINESS, 12); writer.append_u32_le(kLivelinessAutomatic); writer.append_i32_le(kDefaultLeaseDurationSeconds); - writer.append_u32_le(kDefaultLeaseDurationNanoseconds); + writer.append_u32_le(ntp_fraction_from_nanoseconds(kDefaultLeaseDurationNanoseconds)); } void append_parameter_history(ByteWriter &writer) { @@ -587,7 +589,9 @@ std::optional parse_locator(std::span{locator.address.data(), locator.address.size()})) { return std::nullopt; } From 6efb044e33c5c5f65785aad95907f3202fcd2732 Mon Sep 17 00:00:00 2001 From: William Emfinger Date: Wed, 17 Jun 2026 21:31:27 -0500 Subject: [PATCH 21/24] wip improved api --- components/rtps/README.md | 21 ++-- components/rtps/example/main/rtps_example.cpp | 28 +++-- components/rtps/include/rtps.hpp | 28 +++-- components/rtps/src/rtps.cpp | 112 +++++++----------- 4 files changed, 88 insertions(+), 101 deletions(-) diff --git a/components/rtps/README.md b/components/rtps/README.md index c07cd98a0..4fe74aef8 100644 --- a/components/rtps/README.md +++ b/components/rtps/README.md @@ -30,9 +30,12 @@ RTPS separates *metatraffic* from *user traffic*. - **Metatraffic** carries discovery and endpoint metadata. In this component, that means SPDP participant announcements plus SEDP publication and subscription announcements. -- **User traffic** carries application samples. The current ESPP scaffold has a - temporary best-effort `UInt32` user-data path while the standards-based - ROS 2 data plane is still being completed. +- **User traffic** carries application samples. Samples are sent as standard + RTPS `DATA` submessages whose `serializedPayload` is the raw CDR-encapsulated + sample (no ESPP-specific framing); the topic is identified by the writer GUID + resolved through SEDP discovery. The `publish()` / `on_sample` API works with + any CDR-encoded type. Reliable delivery (`HEARTBEAT`/`ACKNACK`) is not yet + implemented, so user traffic is currently best-effort. The current `RtpsParticipant` implementation opens three UDP sockets when `start()` is called: @@ -85,9 +88,9 @@ against every stack". | Peer implementation | Expected compatibility | Notes | | --- | --- | --- | -| ESPP `rtps` component / `python/rtps_host.py` | **Yes** for current scaffold | Intended smoke-test path for SPDP, SEDP, and the temporary `UInt32` `ESPPDATA` user-data payload. | -| Generic DDSI-RTPS 2.3 implementations | **Partial** | SPDP and SEDP messages are standards-shaped, but only the discovery slice is implemented today. | -| ROS 2 nodes backed by Fast DDS | **Partial / discovery-targeted** | The current discovery messages include ROS 2-relevant participant user data such as `enclave=...;`, but standards-based ROS 2 topic data exchange is not finished yet. | +| ESPP `rtps` component / `python/rtps_host.py` | **Yes** for current scaffold | Intended smoke-test path for SPDP, SEDP, and the standard CDR-over-RTPS best-effort user-data path. | +| Generic DDSI-RTPS 2.3 implementations | **Partial** | SPDP, SEDP, and best-effort `DATA` samples are standards-shaped; reliable delivery is not implemented today. | +| ROS 2 nodes backed by Fast DDS | **Partial / discovery-targeted** | Discovery includes ROS 2-relevant participant user data such as `enclave=...;`, and user samples are standard CDR-over-RTPS. Full ROS 2 topic interop additionally needs ROS 2 topic/type name mangling (`rt/...`, `std_msgs::msg::dds_::UInt32_`). | | ROS 2 nodes backed by Cyclone DDS or other DDS vendors | **Partial / unverified** | Expected to be limited to the minimal discovery subset if the peer accepts the currently emitted parameter set; not validated yet. | | Reliable DDS/RTPS endpoints | **No** | `HEARTBEAT`, `ACKNACK`, retransmission windows, and other reliable state-machine pieces are not implemented. | @@ -100,12 +103,12 @@ against every stack". | SPDP participant announce send/receive | **Implemented** | Multicast announce plus participant cache updates. | | SEDP publication / subscription announce send/receive | **Implemented** | Local endpoints are announced and remote endpoints are cached. | | Participant / endpoint discovery callbacks | **Implemented** | Exposed through `on_participant_discovered` and `on_endpoint_discovered`. | -| Temporary `UInt32` user-data path | **Implemented** | Uses the current ESPP-specific `ESPPDATA` payload, not a standards-based DDS sample representation. | +| Standard CDR-over-RTPS user-data path | **Implemented** | `DATA` `serializedPayload` is the raw CDR sample; the `publish()` / `on_sample` API carries any CDR-encoded type, routed by writer GUID via SEDP. | | Best-effort user-data multicast transport | **Implemented** | Supports shared participant-level multicast or endpoint-specific multicast locators advertised in SEDP; local readers only join the multicast groups configured for their topics. | | QoS fields emitted in discovery | **Partial** | Reliability, durability, liveliness, and history parameters are advertised in SEDP. | | QoS matching / policy enforcement | **Not implemented** | Remote QoS is parsed, but full writer/reader matching logic is still missing. | -| Standards-based DDS user-data serialization | **Not implemented** | The current data path is a temporary ESPP scaffold for `std_msgs/msg/UInt32`. | -| Inline QoS handling | **Not implemented** | Discovery and user-data handling assume no inline QoS. | +| ROS 2 topic/type name mangling | **Not implemented** | Topic/type names are emitted verbatim; ROS 2 interop needs `rt/...` topic and `std_msgs::msg::dds_::UInt32_` type mangling. | +| Inline QoS handling | **Partial** | Inline QoS is skipped on receive to reach the payload; it is not emitted or interpreted. | | Reliable RTPS (`HEARTBEAT`, `ACKNACK`, resend`) | **Not implemented** | Reliable delivery is not interoperable yet. | | Full ROS 2 topic interoperability | **Not implemented** | Discovery is the current milestone; ROS 2-compatible data writers/readers are still pending. | diff --git a/components/rtps/example/main/rtps_example.cpp b/components/rtps/example/main/rtps_example.cpp index 3abe97f85..97e03ce8a 100644 --- a/components/rtps/example/main/rtps_example.cpp +++ b/components/rtps/example/main/rtps_example.cpp @@ -151,10 +151,14 @@ extern "C" void app_main(void) { .reliability = espp::RtpsParticipant::ReliabilityKind::BEST_EFFORT, .multicast_group = response_multicast_group, .entity_index = 0, - .on_uint32_sample = - [&logger, &response_count, &last_sent_request](uint32_t value) { + .on_sample = + [&logger, &response_count, &last_sent_request](std::span cdr) { + auto value = espp::RtpsParticipant::deserialize_uint32_cdr(cdr); + if (!value) { + return; + } response_count++; - logger.info("Received response {} (expected {})", value, last_sent_request.load()); + logger.info("Received response {} (expected {})", *value, last_sent_request.load()); }, }); #else @@ -172,12 +176,18 @@ extern "C" void app_main(void) { .reliability = espp::RtpsParticipant::ReliabilityKind::BEST_EFFORT, .multicast_group = request_multicast_group, .entity_index = 0, - .on_uint32_sample = - [&logger, &request_count, &response_topic, &participant_ptr](uint32_t value) { + .on_sample = + [&logger, &request_count, &response_topic, + &participant_ptr](std::span cdr) { + auto value = espp::RtpsParticipant::deserialize_uint32_cdr(cdr); + if (!value) { + return; + } request_count++; - logger.info("Received request {}, sending response", value); - if (!participant_ptr->publish_uint32(response_topic, value)) { - logger.warn("Failed to publish response {}", value); + logger.info("Received request {}, sending response", *value); + if (!participant_ptr->publish(response_topic, + espp::RtpsParticipant::serialize_uint32_cdr(*value))) { + logger.warn("Failed to publish response {}", *value); } }, }); @@ -230,7 +240,7 @@ extern "C" void app_main(void) { auto value = next_request_value.fetch_add(1); last_sent_request = value; - if (participant.publish_uint32(request_topic, value)) { + if (participant.publish(request_topic, espp::RtpsParticipant::serialize_uint32_cdr(value))) { logger.info("Published request {} on '{}'", value, request_topic); } else { logger.warn("Failed to publish request {}", value); diff --git a/components/rtps/include/rtps.hpp b/components/rtps/include/rtps.hpp index 45500ef70..25ff3f873 100644 --- a/components/rtps/include/rtps.hpp +++ b/components/rtps/include/rtps.hpp @@ -168,7 +168,7 @@ class RtpsParticipant : public BaseComponent { ReliabilityKind reliability{ ReliabilityKind::BEST_EFFORT}; ///< Reliability QoS advertised for the writer. std::string multicast_group{}; ///< Optional multicast group advertised for this writer and used - ///< by `publish_uint32()` when set. + ///< by `publish()` when set. uint32_t entity_index{0}; ///< Local entity slot used to derive the RTPS entity ID. }; @@ -181,8 +181,10 @@ class RtpsParticipant : public BaseComponent { std::string multicast_group{}; ///< Optional multicast group advertised for this reader and ///< joined on the standard RTPS user-multicast port when set. uint32_t entity_index{0}; ///< Local entity slot used to derive the RTPS entity ID. - std::function on_uint32_sample{ - nullptr}; ///< Callback invoked when a matching temporary UInt32 sample is received. + std::function)> on_sample{ + nullptr}; ///< Callback invoked with the raw CDR-encapsulated serialized payload + ///< (encapsulation header + body) of a matching received sample. The span is only + ///< valid for the duration of the callback. }; /// @brief Cached information about a discovered remote participant. @@ -332,23 +334,25 @@ class RtpsParticipant : public BaseComponent { /// @return Serialized SEDP subscription message for the reader. std::vector build_sedp_subscription_message(const ReaderConfig &reader_config) const; - /// @brief Build a temporary ESPP UInt32 user-data message. + /// @brief Build a standard RTPS user-data DATA message carrying a CDR-encoded sample. /// @param writer_config Local writer configuration used for the topic and writer entity ID. - /// @param value UInt32 sample value to serialize. - /// @param reliability Reliability flag to encode in the temporary payload header. - /// @return Serialized RTPS DATA message containing the temporary ESPP UInt32 payload. - std::vector build_uint32_data_message(const WriterConfig &writer_config, uint32_t value, - ReliabilityKind reliability) const; + /// @param cdr_payload CDR-encapsulated serialized payload (encapsulation header + body) to carry + /// as the DATA submessage serializedPayload. + /// @return Serialized RTPS DATA message containing the sample. + std::vector build_data_message(const WriterConfig &writer_config, + std::span cdr_payload) const; - /// @brief Publish a temporary ESPP UInt32 sample using the configured user-data transport. + /// @brief Publish a CDR-encoded sample on a topic using the configured user-data transport. /// @param topic_name Topic name to publish on. Must match a registered local writer. - /// @param value UInt32 sample value to send. + /// @param cdr_payload CDR-encapsulated serialized payload (encapsulation header + body) to send. /// @return True if at least one send call succeeded, false otherwise. - bool publish_uint32(std::string_view topic_name, uint32_t value); + bool publish(std::string_view topic_name, std::span cdr_payload); /// @brief Serialize a UInt32 value into a standalone CDR payload. /// @param value Value to serialize. /// @return Encapsulated little-endian CDR payload containing the value. + /// @note Convenience helper for the common ROS 2 `std_msgs/msg/UInt32` case; pair it with + /// `publish()` and `deserialize_uint32_cdr()`. Any CDR-encoded type can be published directly. static std::vector serialize_uint32_cdr(uint32_t value); /// @brief Parse a standalone CDR payload containing a UInt32 value. diff --git a/components/rtps/src/rtps.cpp b/components/rtps/src/rtps.cpp index 319a348f6..719c2ab8f 100644 --- a/components/rtps/src/rtps.cpp +++ b/components/rtps/src/rtps.cpp @@ -12,8 +12,6 @@ namespace { constexpr std::array kRtpsMagic{'R', 'T', 'P', 'S'}; -constexpr std::array kUserDataMagic{'E', 'S', 'P', 'P', 'D', 'A', 'T', 'A'}; -constexpr uint8_t kUserDataVersion = 1; constexpr uint16_t kPortBase = 7400; constexpr uint16_t kDomainGain = 250; @@ -322,24 +320,6 @@ bool parse_ipv4(std::string_view address, std::array &octets) { return true; } -void append_string(ByteWriter &writer, std::string_view text) { - writer.append_u16_le(static_cast(text.size())); - writer.append_bytes( - std::span{reinterpret_cast(text.data()), text.size()}); -} - -std::optional read_string(ByteReader &reader) { - uint16_t length = 0; - if (!reader.read_u16_le(length)) { - return std::nullopt; - } - auto span = reader.read_span(length); - if (span.size() != length) { - return std::nullopt; - } - return std::string(reinterpret_cast(span.data()), span.size()); -} - void append_parameter_header(ByteWriter &writer, ParameterId id, uint16_t length) { writer.append_u16_le(static_cast(id)); writer.append_u16_le(length); @@ -1139,26 +1119,19 @@ RtpsParticipant::build_sedp_subscription_message(const ReaderConfig &reader_conf .serialize(); } -std::vector RtpsParticipant::build_uint32_data_message(const WriterConfig &writer_config, - uint32_t value, - ReliabilityKind reliability) const { - ByteWriter payload_writer; - payload_writer.append_chars(kUserDataMagic); - payload_writer.append_u8(kUserDataVersion); - payload_writer.append_u8(static_cast(reliability)); - append_string(payload_writer, writer_config.topic_name); - auto cdr = serialize_uint32_cdr(value); - payload_writer.append_u16_le(static_cast(cdr.size())); - payload_writer.append_bytes(cdr); - +std::vector +RtpsParticipant::build_data_message(const WriterConfig &writer_config, + std::span cdr_payload) const { + // Standard RTPS: the DATA submessage serializedPayload is exactly the CDR-encapsulated sample. + // The topic is identified by the writer GUID (resolved by the receiver via SEDP discovery), so no + // topic name or other framing is embedded in the payload. auto guid = writer_guid(writer_config.entity_index); return build_message(guid_prefix_, {.value = kEntityIdUnknown}, guid.entity_id, - next_user_data_sequence_number(writer_config.entity_index), - payload_writer.take()) + next_user_data_sequence_number(writer_config.entity_index), cdr_payload) .serialize(); } -bool RtpsParticipant::publish_uint32(std::string_view topic_name, uint32_t value) { +bool RtpsParticipant::publish(std::string_view topic_name, std::span cdr_payload) { WriterConfig writer_config; { std::lock_guard lock(mutex_); @@ -1176,10 +1149,7 @@ bool RtpsParticipant::publish_uint32(std::string_view topic_name, uint32_t value logger_.warn("Reliable user-data retransmission is not implemented yet; sending best-effort"); } - auto encoded_reliability = writer_config.reliability == ReliabilityKind::RELIABLE - ? ReliabilityKind::BEST_EFFORT - : writer_config.reliability; - auto payload = build_uint32_data_message(writer_config, value, encoded_reliability); + auto payload = build_data_message(writer_config, cdr_payload); if (!user_unicast_receiver_) { return false; @@ -1471,48 +1441,48 @@ bool RtpsParticipant::handle_user_message(std::vector &data, const Sock for (const auto &submessage : message->submessages) { bool valid_data = false; auto data_view = parse_data_submessage(submessage, valid_data); - if (!valid_data || data_view.serialized_payload.size() < kUserDataMagic.size() + 2 || - !std::equal(kUserDataMagic.begin(), kUserDataMagic.end(), - data_view.serialized_payload.begin())) { - continue; - } - - ByteReader reader(data_view.serialized_payload); - std::array magic{}; - uint8_t version = 0; - uint8_t reliability = 0; - if (!reader.read_bytes(std::span{magic.data(), magic.size()}) || - !reader.read_u8(version) || !reader.read_u8(reliability)) { - continue; - } - auto topic_name = read_string(reader); - uint16_t payload_length = 0; - if (version != kUserDataVersion || !topic_name || !reader.read_u16_le(payload_length)) { - continue; - } - auto payload = reader.read_span(payload_length); - auto maybe_value = deserialize_uint32_cdr(payload); - if (!maybe_value) { + if (!valid_data) { continue; } - if (static_cast(reliability) == ReliabilityKind::RELIABLE) { - logger_.warn( - "Received reliable topic '{}' from {}, but ACKNACK/HEARTBEAT is not implemented yet", - *topic_name, sender); - } - - std::vector> callbacks; + // Standard RTPS: the sample's topic is identified by the writer GUID, which we resolve through + // SEDP discovery state. Build the remote writer GUID from the message prefix + DATA writerId, + // then look up its topic and reliability among discovered writers, and collect the matching + // local reader callbacks under a single lock. + Guid remote_writer_guid{.prefix = message->header.guid_prefix, + .entity_id = data_view.writer_id}; + std::string topic_name; + bool writer_is_reliable = false; + std::vector)>> callbacks; { std::lock_guard lock(mutex_); + auto writer = std::find_if( + discovered_writers_.begin(), discovered_writers_.end(), + [&remote_writer_guid](const auto &w) { return w.guid == remote_writer_guid; }); + if (writer == discovered_writers_.end()) { + // Sample arrived before the writer was discovered via SEDP; drop it (best-effort). + continue; + } + topic_name = writer->topic_name; + writer_is_reliable = writer->reliability == ReliabilityKind::RELIABLE; for (const auto &reader_config : readers_) { - if (reader_config.topic_name == *topic_name && reader_config.on_uint32_sample) { - callbacks.push_back(reader_config.on_uint32_sample); + if (reader_config.topic_name == topic_name && reader_config.on_sample) { + callbacks.push_back(reader_config.on_sample); } } } + + if (callbacks.empty()) { + continue; + } + if (writer_is_reliable) { + logger_.warn("Received sample on reliable topic '{}' from {}, but ACKNACK/HEARTBEAT is not " + "implemented " + "yet", + topic_name, sender); + } for (const auto &callback : callbacks) { - callback(*maybe_value); + callback(data_view.serialized_payload); } } return false; From 58bce6d40d490e0439c342f5559041e14d2c6d32 Mon Sep 17 00:00:00 2001 From: William Emfinger Date: Thu, 18 Jun 2026 09:03:39 -0500 Subject: [PATCH 22/24] clean up some more and fix some issues --- components/rtps/example/main/rtps_example.cpp | 36 ++- components/rtps/include/rtps.hpp | 12 - components/rtps/src/rtps.cpp | 21 -- python/README.md | 8 +- python/rtps_host.py | 227 ++++++++++++------ 5 files changed, 186 insertions(+), 118 deletions(-) diff --git a/components/rtps/example/main/rtps_example.cpp b/components/rtps/example/main/rtps_example.cpp index 97e03ce8a..b5d9217bf 100644 --- a/components/rtps/example/main/rtps_example.cpp +++ b/components/rtps/example/main/rtps_example.cpp @@ -1,8 +1,13 @@ #include #include #include +#include +#include +#include #include +#include +#include "cdr.hpp" #include "logger.hpp" #include "rtps.hpp" #include "wifi_sta.hpp" @@ -12,6 +17,24 @@ using namespace std::chrono_literals; namespace { constexpr std::string_view kTypeName = "std_msgs/msg/UInt32"; +// Thin wrappers over the `cdr` component for the std_msgs/msg/UInt32 payload used by this example. +// The rtps `publish()` / `on_sample` API works with any CDR-encoded type, so serialization lives in +// the application rather than the participant. +std::vector serialize_uint32(uint32_t value) { + espp::CdrWriter writer; + writer.write(value); + return writer.take_buffer(); +} + +std::optional deserialize_uint32(std::span cdr) { + espp::CdrReader reader(cdr); + uint32_t value = 0; + if (!reader.valid() || !reader.read(value)) { + return std::nullopt; + } + return value; +} + bool run_local_protocol_checks(espp::Logger &logger, const espp::RtpsParticipant &participant) { auto announce_message = participant.build_announce_message(); auto parsed_message = espp::RtpsParticipant::Message::parse(announce_message); @@ -48,8 +71,8 @@ bool run_local_protocol_checks(espp::Logger &logger, const espp::RtpsParticipant parsed_subscription_message->submessages.size()); } - auto uint32_payload = espp::RtpsParticipant::serialize_uint32_cdr(42); - auto maybe_value = espp::RtpsParticipant::deserialize_uint32_cdr(uint32_payload); + auto uint32_payload = serialize_uint32(42); + auto maybe_value = deserialize_uint32(uint32_payload); if (!maybe_value || *maybe_value != 42) { logger.error("UInt32 CDR round trip failed"); return false; @@ -153,7 +176,7 @@ extern "C" void app_main(void) { .entity_index = 0, .on_sample = [&logger, &response_count, &last_sent_request](std::span cdr) { - auto value = espp::RtpsParticipant::deserialize_uint32_cdr(cdr); + auto value = deserialize_uint32(cdr); if (!value) { return; } @@ -179,14 +202,13 @@ extern "C" void app_main(void) { .on_sample = [&logger, &request_count, &response_topic, &participant_ptr](std::span cdr) { - auto value = espp::RtpsParticipant::deserialize_uint32_cdr(cdr); + auto value = deserialize_uint32(cdr); if (!value) { return; } request_count++; logger.info("Received request {}, sending response", *value); - if (!participant_ptr->publish(response_topic, - espp::RtpsParticipant::serialize_uint32_cdr(*value))) { + if (!participant_ptr->publish(response_topic, serialize_uint32(*value))) { logger.warn("Failed to publish response {}", *value); } }, @@ -240,7 +262,7 @@ extern "C" void app_main(void) { auto value = next_request_value.fetch_add(1); last_sent_request = value; - if (participant.publish(request_topic, espp::RtpsParticipant::serialize_uint32_cdr(value))) { + if (participant.publish(request_topic, serialize_uint32(value))) { logger.info("Published request {} on '{}'", value, request_topic); } else { logger.warn("Failed to publish request {}", value); diff --git a/components/rtps/include/rtps.hpp b/components/rtps/include/rtps.hpp index 25ff3f873..4fcdddb86 100644 --- a/components/rtps/include/rtps.hpp +++ b/components/rtps/include/rtps.hpp @@ -348,18 +348,6 @@ class RtpsParticipant : public BaseComponent { /// @return True if at least one send call succeeded, false otherwise. bool publish(std::string_view topic_name, std::span cdr_payload); - /// @brief Serialize a UInt32 value into a standalone CDR payload. - /// @param value Value to serialize. - /// @return Encapsulated little-endian CDR payload containing the value. - /// @note Convenience helper for the common ROS 2 `std_msgs/msg/UInt32` case; pair it with - /// `publish()` and `deserialize_uint32_cdr()`. Any CDR-encoded type can be published directly. - static std::vector serialize_uint32_cdr(uint32_t value); - - /// @brief Parse a standalone CDR payload containing a UInt32 value. - /// @param data Encapsulated CDR payload bytes. - /// @return Parsed UInt32 value on success, or `std::nullopt` if the payload is invalid. - static std::optional deserialize_uint32_cdr(std::span data); - /// @brief Compute the standard RTPS UDP port mapping for a domain/participant pair. /// @param domain_id RTPS domain ID. /// @param participant_id RTPS participant ID. diff --git a/components/rtps/src/rtps.cpp b/components/rtps/src/rtps.cpp index 719c2ab8f..2a61530a3 100644 --- a/components/rtps/src/rtps.cpp +++ b/components/rtps/src/rtps.cpp @@ -1189,27 +1189,6 @@ int64_t RtpsParticipant::next_user_data_sequence_number(uint32_t entity_index) c return current; } -std::vector RtpsParticipant::serialize_uint32_cdr(uint32_t value) { - espp::CdrWriter writer({ - .encapsulation = espp::CdrEncapsulation::CDR_LE, - .include_encapsulation = true, - }); - writer.write(value); - return writer.take_buffer(); -} - -std::optional RtpsParticipant::deserialize_uint32_cdr(std::span data) { - espp::CdrReader reader(data); - if (!reader.valid()) { - return std::nullopt; - } - uint32_t value = 0; - if (!reader.read(value)) { - return std::nullopt; - } - return value; -} - RtpsParticipant::PortMapping RtpsParticipant::compute_port_mapping(uint16_t domain_id, uint16_t participant_id) { auto base = static_cast(kPortBase) + static_cast(kDomainGain) * domain_id; diff --git a/python/README.md b/python/README.md index 9ea454864..c54b698e6 100644 --- a/python/README.md +++ b/python/README.md @@ -49,9 +49,11 @@ This section gives a brief overview of what the scripts in this folder do. window. - `rtps_host.py`: A pure-stdlib host-side RTPS harness for discovering an ESPP `RtpsParticipant`, printing SPDP/SEDP metadata, and optionally publishing or - receiving the current temporary `UInt32` user-data payloads without needing - Python bindings. It now follows endpoint-advertised user-data multicast - locators, joining matching subscribed-topic multicast groups dynamically. + receiving standard CDR-over-RTPS `UInt32` user-data samples (routed by writer + GUID via SEDP) without needing Python bindings. It follows endpoint-advertised + user-data multicast locators, joining matching subscribed-topic multicast + groups dynamically. Run `python rtps_host.py --self-test` to validate the + wire-format encoders/decoders against the firmware with no network I/O. - `cobs_demo.py`: Demonstration of ESPP COBS functionality with native Python data types. Shows ESPP encoding/decoding, cross-library compatibility with the cobs-python library, and practical usage examples. Includes design differences explanation and validation diff --git a/python/rtps_host.py b/python/rtps_host.py index 5cd6d4beb..533b5d3c8 100644 --- a/python/rtps_host.py +++ b/python/rtps_host.py @@ -1,14 +1,18 @@ #!/usr/bin/env python3 """Simple host-side RTPS test harness for the ESPP RTPS component. -This script speaks the current ESPP RTPS discovery wire format plus the -temporary ``UInt32`` user-data payload used by ``RtpsParticipant`` today. It is -useful for: +This script speaks the ESPP RTPS discovery wire format plus the standard +CDR-over-RTPS user-data path used by ``RtpsParticipant``. It is useful for: 1. discovering an embedded ESPP RTPS participant from a PC/host, 2. inspecting SPDP/SEDP announcements, and -3. sending or receiving ``std_msgs/msg/UInt32``-style test samples over the - temporary ESPP user-data path. +3. sending or receiving ``std_msgs/msg/UInt32``-style test samples. User samples + are standard RTPS ``DATA`` submessages whose serializedPayload is the raw + CDR-encapsulated sample; the topic is identified by the writer GUID resolved + through SEDP discovery (no ESPP-specific payload framing). + +Run ``python rtps_host.py --self-test`` to validate the wire-format encoders and +decoders against the firmware's expectations without any network I/O. It uses only the Python standard library, so it does not require Python bindings or a rebuilt host ``lib/`` tree. @@ -30,8 +34,6 @@ RTPS_MAGIC = b"RTPS" PL_CDR_LE = b"\x00\x03\x00\x00" -USER_DATA_MAGIC = b"ESPPDATA" -USER_DATA_VERSION = 1 PORT_BASE = 7400 DOMAIN_GAIN = 250 @@ -47,8 +49,6 @@ RTPS_QOS_RELIABILITY_BEST_EFFORT = 1 RTPS_QOS_RELIABILITY_RELIABLE = 2 -USER_DATA_RELIABILITY_BEST_EFFORT = 0 -USER_DATA_RELIABILITY_RELIABLE = 1 KIND_UDP_V4 = 1 VENDOR_ID = b"\xca\xfe" @@ -179,12 +179,14 @@ def reliability_to_name(reliable: bool) -> str: return "reliable" if reliable else "best-effort" -def encode_user_data_reliability(reliable: bool) -> int: - return USER_DATA_RELIABILITY_RELIABLE if reliable else USER_DATA_RELIABILITY_BEST_EFFORT +def ntp_fraction_from_nanoseconds(nanoseconds: int) -> int: + # RTPS Duration_t/Time_t use NTP fraction units of 1/2^32 s, not nanoseconds. + return (nanoseconds << 32) // 1_000_000_000 -def decode_user_data_reliability(encoded: int) -> str: - return "reliable" if encoded == USER_DATA_RELIABILITY_RELIABLE else "best-effort" +def padded_parameter_length(length: int) -> int: + # RTPS PL_CDR requires each parameterLength to be a multiple of 4. + return (length + 3) & ~3 def compute_port_mapping(domain_id: int, participant_id: int) -> PortMapping: @@ -262,13 +264,14 @@ def append_parameter_bool(buffer: bytearray, pid: int, value: bool) -> None: def append_parameter_duration(buffer: bytearray, pid: int, seconds: int, nanoseconds: int) -> None: append_parameter_header(buffer, pid, 8) - buffer.extend(struct.pack(" bytes: + # Locator_t.kind/.port are little-endian in PL_CDR_LE; only the 16-byte address is raw bytes. locator = bytearray(24) - struct.pack_into(">I", locator, 0, KIND_UDP_V4) - struct.pack_into(">I", locator, 4, port) + struct.pack_into(" None: encoded = text.encode("utf-8") - append_parameter_header(buffer, pid, 4 + len(encoded) + 1) + # parameterLength must be a multiple of 4 and includes the trailing CDR padding. + append_parameter_header(buffer, pid, padded_parameter_length(4 + len(encoded) + 1)) buffer.extend(struct.pack(" None: def append_parameter_octet_sequence(buffer: bytearray, pid: int, payload: bytes) -> None: - append_parameter_header(buffer, pid, 4 + len(payload)) + append_parameter_header(buffer, pid, padded_parameter_length(4 + len(payload))) buffer.extend(struct.pack(" None: append_parameter_header(buffer, PID_RELIABILITY, 12) kind = RTPS_QOS_RELIABILITY_RELIABLE if reliable else RTPS_QOS_RELIABILITY_BEST_EFFORT buffer.extend(struct.pack(" None: @@ -309,7 +319,13 @@ def append_parameter_durability(buffer: bytearray) -> None: def append_parameter_liveliness(buffer: bytearray) -> None: append_parameter_header(buffer, PID_LIVELINESS, 12) buffer.extend(struct.pack(" None: @@ -423,10 +439,10 @@ def parse_octet_sequence(value: Optional[bytes]) -> Optional[bytes]: def parse_locator(value: Optional[bytes]) -> tuple[str, int]: if value is None or len(value) != 24: return ("0.0.0.0", 0) - kind = struct.unpack_from(">I", value, 0)[0] + kind = struct.unpack_from("I", value, 4)[0] + port = struct.unpack_from(" None: self.next_discovery_send = 0.0 self.next_publish_send = 0.0 self.last_no_participant_log = 0.0 + self.last_unknown_writer_log = 0.0 def _create_bound_udp_socket(self, port: int) -> socket.socket: sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM, socket.IPPROTO_UDP) @@ -548,7 +565,12 @@ def _create_user_multicast_socket(self) -> socket.socket: # Some platforms expose SO_REUSEPORT but reject setting it; this is # a best-effort optimization and is not required for correctness. pass - sock.bind((self.args.bind_address, self.ports.user_multicast)) + # Bind to INADDR_ANY, not a unicast address: multicast datagrams are addressed to the group + # (e.g. 239.255.0.11), so a socket bound to a specific unicast interface address will not + # receive them on Linux (and unreliably on macOS). Delivery is decided by the joined + # group(s) + port. This socket may join several user-data groups, so binding to a single + # group address is not an option. + sock.bind(("", self.ports.user_multicast)) sock.setblocking(False) return sock @@ -683,24 +705,15 @@ def build_sedp_subscription_message(self, reader: ReaderConfig) -> bytes: payload, ) - def build_uint32_data_message(self, writer: WriterConfig, value: int) -> bytes: - payload = bytearray() - payload.extend(USER_DATA_MAGIC) - payload.append(USER_DATA_VERSION) - payload.append(encode_user_data_reliability(writer.reliable)) - topic_name = writer.topic_name.encode("utf-8") - payload.extend(struct.pack(" bytes: + # Standard RTPS: the DATA serializedPayload is exactly the CDR-encapsulated sample. writer_entity_id = entity_id_for_index(writer.entity_index, USER_WRITER_NO_KEY_KIND) return build_rtps_message( self.guid_prefix, ENTITY_ID_UNKNOWN, writer_entity_id, self._next_sequence(writer_entity_id), - bytes(payload), + cdr_payload, ) def send_spdp_announce_now(self) -> None: @@ -765,7 +778,7 @@ def _build_user_targets(self, writer: WriterConfig) -> List[Tuple[str, int]]: return targets def _publish_value(self, writer: WriterConfig, value: int, target: Optional[tuple[str, int]] = None) -> bool: - payload = self.build_uint32_data_message(writer, value) + payload = self.build_data_message(writer, serialize_uint32_cdr(value)) if target is not None: self.user_unicast_sock.sendto(payload, target) return True @@ -777,7 +790,7 @@ def _publish_value(self, writer: WriterConfig, value: int, target: Optional[tupl return True def handle_metatraffic_packet(self, packet: bytes, sender_ip: str) -> None: - for writer_id, serialized_payload in parse_rtps_data_messages(packet): + for _guid_prefix, writer_id, serialized_payload in parse_rtps_data_messages(packet): parameters = parse_parameter_list(serialized_payload) if not parameters: continue @@ -873,49 +886,43 @@ def _handle_sedp(self, parameters: List[tuple[int, bytes]], sender_ip: str, is_r ) def handle_user_packet(self, packet: bytes, sender_ip: str, sender_port: int) -> None: - for writer_id, serialized_payload in parse_rtps_data_messages(packet): - if not serialized_payload.startswith(USER_DATA_MAGIC) or len(serialized_payload) < len(USER_DATA_MAGIC) + 2: - continue - offset = len(USER_DATA_MAGIC) - version = serialized_payload[offset] - reliability = serialized_payload[offset + 1] - offset += 2 - if version != USER_DATA_VERSION: - continue - if offset + 2 > len(serialized_payload): - continue - topic_length = struct.unpack_from(" len(serialized_payload): - continue - topic_name = serialized_payload[offset : offset + topic_length].decode("utf-8", errors="replace") - offset += topic_length - if offset + 2 > len(serialized_payload): + subscribed_topics = {reader.topic_name for reader in self.local_readers} + for guid_prefix, writer_id, serialized_payload in parse_rtps_data_messages(packet): + # Standard RTPS: resolve the topic from the writer GUID via SEDP discovery state. + writer_guid = guid_prefix + writer_id + writer = self.discovered_writers.get(writer_guid) + if writer is None: + # Sample arrived before its writer was discovered via SEDP; drop it (best-effort). + # Surface it (rate-limited) so a missing SEDP exchange is visible rather than silent. + now = time.monotonic() + if now - self.last_unknown_writer_log > 2.0: + log( + f"[data] received {len(serialized_payload)}-byte sample from UNDISCOVERED " + f"writer {guid_to_string(writer_guid)} at {sender_ip}:{sender_port}; cannot " + f"route without SEDP (discovered_writers={len(self.discovered_writers)})" + ) + self.last_unknown_writer_log = now continue - payload_length = struct.unpack_from(" len(serialized_payload): + topic_name = writer.topic_name + if topic_name not in subscribed_topics: continue - maybe_value = deserialize_uint32_cdr(serialized_payload[offset : offset + payload_length]) + maybe_value = deserialize_uint32_cdr(serialized_payload) if maybe_value is None: continue - reliability_name = decode_user_data_reliability(reliability) log( - f"[data] topic='{topic_name}' value={maybe_value} reliability={reliability_name} " + f"[data] topic='{topic_name}' value={maybe_value} reliability={writer.reliability} " f"from {sender_ip}:{sender_port} writer={hex_string(writer_id)}" ) if self.args.echo_received and self.local_writers: - subscribed_topics = {reader.topic_name for reader in self.local_readers} - if topic_name in subscribed_topics: - writer = self.local_writers[0] - if self._publish_value(writer, maybe_value): - log(f"[echo] responded with value={maybe_value} on '{writer.topic_name}'") - else: - self._publish_value(writer, maybe_value, (sender_ip, sender_port)) - log( - f"[echo] responded with value={maybe_value} on '{writer.topic_name}' " - f"to {sender_ip}:{sender_port}" - ) + out_writer = self.local_writers[0] + if self._publish_value(out_writer, maybe_value): + log(f"[echo] responded with value={maybe_value} on '{out_writer.topic_name}'") + else: + self._publish_value(out_writer, maybe_value, (sender_ip, sender_port)) + log( + f"[echo] responded with value={maybe_value} on '{out_writer.topic_name}' " + f"to {sender_ip}:{sender_port}" + ) def run(self) -> None: start_time = time.monotonic() @@ -990,11 +997,13 @@ def close(self) -> None: log(f"[close] ignoring socket close failure for {sock!r}: {exc}") -def parse_rtps_data_messages(packet: bytes) -> List[tuple[bytes, bytes]]: +def parse_rtps_data_messages(packet: bytes) -> List[tuple[bytes, bytes, bytes]]: + """Return (guid_prefix, writer_id, serialized_payload) for each DATA submessage.""" if len(packet) < 20 or not packet.startswith(RTPS_MAGIC): return [] + guid_prefix = packet[8:20] offset = 20 - messages: List[tuple[bytes, bytes]] = [] + messages: List[tuple[bytes, bytes, bytes]] = [] while offset + 4 <= len(packet): kind = packet[offset] flags = packet[offset + 1] @@ -1014,10 +1023,75 @@ def parse_rtps_data_messages(packet: bytes) -> List[tuple[bytes, bytes]]: continue writer_id = payload[8:12] serialized_payload = payload[20:] - messages.append((writer_id, serialized_payload)) + messages.append((guid_prefix, writer_id, serialized_payload)) return messages +def run_self_test() -> int: + """Validate the wire-format encoders/decoders against firmware expectations (no network I/O).""" + failures: List[str] = [] + + def check(name: str, condition: bool) -> None: + log(f" [{'PASS' if condition else 'FAIL'}] {name}") + if not condition: + failures.append(name) + + # Locators: kind/port are little-endian, address is raw network-order bytes; round-trips. + loc = locator_bytes("192.168.1.5", 7411) + check("locator kind is little-endian", loc[:4] == struct.pack(" 16 + append_parameter_string_cdr(params, PID_TYPE_NAME, "std_msgs::msg::dds_::UInt32_") + append_parameter_octet_sequence(params, PID_USER_DATA, b"enclave=/;") # body 4+10=14 -> 16 + offset = 0 + aligned = True + while offset + 4 <= len(params): + pid, length = struct.unpack_from(" argparse.Namespace: parser = argparse.ArgumentParser( description=( @@ -1115,6 +1189,9 @@ def parse_args() -> argparse.Namespace: def main() -> int: + # Handle --self-test before full argument parsing so it needs no network/address configuration. + if "--self-test" in sys.argv: + return run_self_test() args = parse_args() harness = RtpsHostHarness(args) harness.run() From 578de256e194fc30479723a6fca80796058cdefe Mon Sep 17 00:00:00 2001 From: William Emfinger Date: Thu, 18 Jun 2026 09:10:38 -0500 Subject: [PATCH 23/24] make the crd de-/serialization format clearer for the example --- components/rtps/example/main/rtps_example.cpp | 14 ++++++++++++-- 1 file changed, 12 insertions(+), 2 deletions(-) diff --git a/components/rtps/example/main/rtps_example.cpp b/components/rtps/example/main/rtps_example.cpp index b5d9217bf..a18a56fd4 100644 --- a/components/rtps/example/main/rtps_example.cpp +++ b/components/rtps/example/main/rtps_example.cpp @@ -21,13 +21,23 @@ constexpr std::string_view kTypeName = "std_msgs/msg/UInt32"; // The rtps `publish()` / `on_sample` API works with any CDR-encoded type, so serialization lives in // the application rather than the participant. std::vector serialize_uint32(uint32_t value) { - espp::CdrWriter writer; + // The encapsulation options below are the CdrWriter defaults (little-endian CDR with a 4-byte + // encapsulation header); shown explicitly for clarity about the on-the-wire format. + espp::CdrWriter writer({ + .encapsulation = espp::CdrEncapsulation::CDR_LE, + .include_encapsulation = true, + }); writer.write(value); return writer.take_buffer(); } std::optional deserialize_uint32(std::span cdr) { - espp::CdrReader reader(cdr); + // The config below matches the CdrReader defaults (expects a little-endian CDR encapsulation + // header); shown explicitly to mirror serialize_uint32() above. + espp::CdrReader reader(cdr, { + .expect_encapsulation = true, + .default_encapsulation = espp::CdrEncapsulation::CDR_LE, + }); uint32_t value = 0; if (!reader.valid() || !reader.read(value)) { return std::nullopt; From 10d216e3664653cb401709be6f390352ea684155 Mon Sep 17 00:00:00 2001 From: William Emfinger Date: Thu, 18 Jun 2026 09:18:53 -0500 Subject: [PATCH 24/24] improve hash function --- components/rtps/src/rtps.cpp | 18 +++++++++++++++++- 1 file changed, 17 insertions(+), 1 deletion(-) diff --git a/components/rtps/src/rtps.cpp b/components/rtps/src/rtps.cpp index 2a61530a3..171efb49d 100644 --- a/components/rtps/src/rtps.cpp +++ b/components/rtps/src/rtps.cpp @@ -93,6 +93,19 @@ enum class ParameterId : uint16_t { PID_HISTORY = 0x0040, }; +// 64-bit FNV-1a hash. Used to derive the node-name portion of the GUID prefix with a full 64 bits +// of entropy regardless of the platform's size_t width. std::hash is only 32-bit on +// the 32-bit ESP32, which made bytes 8..11 of the prefix a repeated copy of bytes 4..7 (the shift +// `hash >> (8 * i)` for i >= 4 was undefined behavior on a 32-bit value). +uint64_t fnv1a_64(std::string_view text) { + uint64_t hash = 1469598103934665603ull; // FNV-1a 64-bit offset basis + for (unsigned char c : text) { + hash ^= c; + hash *= 1099511628211ull; // FNV-1a 64-bit prime + } + return hash; +} + class ByteWriter { public: void append_bytes(std::span bytes) { @@ -818,7 +831,10 @@ RtpsParticipant::Message::parse(std::span data) { RtpsParticipant::RtpsParticipant(const Config &config) : BaseComponent({.tag = "RtpsParticipant", .level = config.log_level}) , config_(config) { - auto hash = std::hash{}(config_.node_name); + // GUID prefix layout: bytes 0..1 = participant_id, 2..3 = domain_id, 4..11 = 64-bit node-name + // hash. Uniqueness across participants on one host relies on distinct participant_ids; the + // node-name hash distinguishes different nodes/applications. + uint64_t hash = fnv1a_64(config_.node_name); guid_prefix_.value[0] = config_.participant_id & 0xff; guid_prefix_.value[1] = (config_.participant_id >> 8) & 0xff; guid_prefix_.value[2] = config_.domain_id & 0xff;