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..4942ce15e --- /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}; + + //! [cdr example] + espp::CdrWriter writer({ + .encapsulation = espp::CdrEncapsulation::CDR_LE, + .include_encapsulation = true, + }); + 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); + //! [cdr example] + + 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..1dff9d53d --- /dev/null +++ b/components/cdr/include/cdr.hpp @@ -0,0 +1,547 @@ +#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. + /// @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)); + } + + /// @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. + /// @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 true; + } + while (data_.size() % alignment != 0) { + data_.push_back(0); + } + return true; + } + + /// @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) { + 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. + /// @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) { + constexpr size_t alignment = detail::cdr_alignment(); + if constexpr (alignment > 1) { + if (!align(alignment)) { // cppcheck-suppress knownConditionTrueFalse + valid_ = false; + return false; + } + } + if (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; + // 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 + // when there are bytes left to consume so a valid final string is not rejected. + if (remaining() == 0) { + return true; + } + 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; + } + // 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(); + 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..4fe74aef8 --- /dev/null +++ b/components/rtps/README.md @@ -0,0 +1,119 @@ +# 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 +- 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 +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. 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: + +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 +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 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. | + +## 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`. | +| 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. | +| 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. | + +## 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..0c2183c98 --- /dev/null +++ b/components/rtps/example/README.md @@ -0,0 +1,77 @@ +# 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. 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: + +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` + +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/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..7d4a64dbc --- /dev/null +++ b/components/rtps/example/main/Kconfig.projbuild @@ -0,0 +1,109 @@ +menu "RTPS Example Configuration" + + choice RTPS_EXAMPLE_ROLE + prompt "Example Role" + default RTPS_EXAMPLE_ROLE_INITIATOR + help + Build one board as the initiator and a second board as the responder + to verify RTPS discovery and end-to-end UInt32 sample exchange. + + config RTPS_EXAMPLE_ROLE_INITIATOR + bool "Initiator" + help + Publishes incrementing request samples after it discovers a + responder, then logs the matching responses. + + config RTPS_EXAMPLE_ROLE_RESPONDER + bool "Responder" + help + Waits for request samples and echoes the same value back on the + response topic. + + endchoice + + config RTPS_EXAMPLE_NODE_NAME + string "Participant node name" + default "espp_rtps_initiator" if RTPS_EXAMPLE_ROLE_INITIATOR + default "espp_rtps_responder" if RTPS_EXAMPLE_ROLE_RESPONDER + help + Logical RTPS participant name announced during discovery. + + config RTPS_EXAMPLE_DOMAIN_ID + int "RTPS domain ID" + range 0 231 + default 0 + help + Both boards must use the same domain ID to discover each other. + + config RTPS_EXAMPLE_PARTICIPANT_ID + int "RTPS participant ID" + range 0 119 + default 1 if RTPS_EXAMPLE_ROLE_INITIATOR + default 2 if RTPS_EXAMPLE_ROLE_RESPONDER + help + Each board should use a unique participant ID within the same domain. + + config RTPS_EXAMPLE_TOPIC_PREFIX + string "Topic prefix" + default "espp/rtps_example" + help + Prefix used to derive the request and response topics. + + config RTPS_EXAMPLE_ANNOUNCE_PERIOD_MS + int "Discovery announce period (ms)" + range 200 10000 + default 1500 + help + Period between periodic SPDP/SEDP discovery announcements. + + config RTPS_EXAMPLE_PUBLISH_PERIOD_MS + int "Initiator publish period (ms)" + range 250 60000 + default 2000 + depends on RTPS_EXAMPLE_ROLE_INITIATOR + help + Period between request messages sent by the initiator after a + responder has been discovered. + + config RTPS_EXAMPLE_USE_USER_MULTICAST + bool "Use best-effort user-data multicast" + default n + help + If enabled, the example advertises and uses topic-specific + multicast groups for the request and response topics instead of + per-participant unicast delivery. + + config RTPS_EXAMPLE_REQUEST_MULTICAST_GROUP + string "Request topic multicast group" + default "239.255.0.11" + depends on RTPS_EXAMPLE_USE_USER_MULTICAST + help + IPv4 multicast group used for the /request topic. + + config RTPS_EXAMPLE_RESPONSE_MULTICAST_GROUP + string "Response topic multicast group" + default "239.255.0.12" + depends on RTPS_EXAMPLE_USE_USER_MULTICAST + help + IPv4 multicast group used for the /response topic. + + config ESP_WIFI_SSID + string "WiFi SSID" + default "" + help + SSID (network name) for the example to connect to. + + config ESP_WIFI_PASSWORD + string "WiFi Password" + default "" + help + WiFi password (WPA or WPA2) for the example to use. + + config ESP_MAXIMUM_RETRY + int "Maximum retry" + default 5 + help + Set the maximum retry count to avoid reconnecting forever when the + network is unavailable. + +endmenu diff --git a/components/rtps/example/main/rtps_example.cpp b/components/rtps/example/main/rtps_example.cpp new file mode 100644 index 000000000..a18a56fd4 --- /dev/null +++ b/components/rtps/example/main/rtps_example.cpp @@ -0,0 +1,291 @@ +#include +#include +#include +#include +#include +#include +#include +#include + +#include "cdr.hpp" +#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"; + +// 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) { + // 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) { + // 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; + } + 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); + 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 = 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; + } + logger.info("UInt32 CDR round trip succeeded with value {}", *maybe_value); + return true; +} + +[[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; + }); +} +} // namespace + +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, + .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"; +#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}; + std::atomic next_request_value{1}; + std::atomic last_sent_request{0}; + + 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, + // 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 + participant.add_writer({ + .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_sample = + [&logger, &response_count, &last_sent_request](std::span cdr) { + auto value = deserialize_uint32(cdr); + if (!value) { + return; + } + response_count++; + logger.info("Received response {} (expected {})", *value, last_sent_request.load()); + }, + }); +#else + auto *participant_ptr = &participant; + participant.add_writer({ + .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_sample = + [&logger, &request_count, &response_topic, + &participant_ptr](std::span cdr) { + auto value = deserialize_uint32(cdr); + if (!value) { + return; + } + request_count++; + logger.info("Received request {}, sending response", *value); + if (!participant_ptr->publish(response_topic, serialize_uint32(*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 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; + } + + if (!participant.start()) { + 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..."); + 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(request_topic, serialize_uint32(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..4fcdddb86 --- /dev/null +++ b/components/rtps/include/rtps.hpp @@ -0,0 +1,400 @@ +#pragma once + +#include +#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. +/// +/// \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. + 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. + std::string multicast_group{}; ///< Optional multicast group advertised for this writer and used + ///< by `publish()` 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. + 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. + 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_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. + 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. + std::vector multicast_locators{}; ///< Multicast locators advertised by the endpoint + ///< for user-data traffic. + }; + + /// @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. + 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. + 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. + 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. + /// @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 snapshot copy of the local writer list. + std::vector writers() const; + + /// @brief Access the registered local reader configurations. + /// @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 + /// 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 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 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 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 cdr_payload CDR-encapsulated serialized payload (encapsulation header + body) to send. + /// @return True if at least one send call succeeded, false otherwise. + bool publish(std::string_view topic_name, std::span cdr_payload); + + /// @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: + 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(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; + 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(); + + Config config_; + GuidPrefix guid_prefix_{}; + std::atomic_bool started_{false}; + + 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_; + + 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}; + 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_; + 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..171efb49d --- /dev/null +++ b/components/rtps/src/rtps.cpp @@ -0,0 +1,1681 @@ +#include "rtps.hpp" + +#include +#include +#include +#include +#include +#include +#include + +#include "cdr.hpp" + +namespace { +constexpr std::array kRtpsMagic{'R', 'T', 'P', 'S'}; + +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; +// 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 { + 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, +}; + +// 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) { + 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_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_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_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; + } + 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(int64_t &value, bool little_endian) { + uint32_t high = 0; + uint32_t low = 0; + 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; + } + offset_ += length; + 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_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); +} + +// 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(ntp_fraction_from_nanoseconds(nanoseconds)); +} + +void append_parameter_locator(ByteWriter &writer, ParameterId id, + const espp::RtpsParticipant::Locator &locator) { + append_parameter_header(writer, id, 24); + // 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); +} + +void append_parameter_string_cdr(ByteWriter &writer, ParameterId id, std::string_view text) { + auto cdr_writer = espp::CdrWriter::make_body_writer(espp::CdrEncapsulation::CDR_LE); + cdr_writer.write_string(text); + 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) { + 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); + 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, + 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(ntp_fraction_from_nanoseconds(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(ntp_fraction_from_nanoseconds(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); + // 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; + } + + 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::vector find_parameters(std::span parameters, + ParameterId id) { + std::vector matches; + std::copy_if(parameters.begin(), parameters.end(), std::back_inserter(matches), + [id](const auto ¶meter) { return parameter.id == id; }); + return matches; +} + +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; + } + 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 std::string(reinterpret_cast(text_bytes.data()), text_bytes.size() - 1); +} + +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; + // kind and port are little-endian in PL_CDR_LE (see append_parameter_locator); the address is a + // raw 16-byte field read verbatim. + if (!reader.read_u32_le(kind) || !reader.read_u32_le(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; +} + +bool has_valid_locator(const espp::RtpsParticipant::Locator &locator) { + return locator.kind == espp::RtpsParticipant::Locator::Kind::UDP_V4 && locator.port != 0 && + std::any_of(locator.address.begin() + 12, locator.address.end(), + [](uint8_t octet) { return octet != 0; }); +} + +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(); + } + // Normalize an empty enclave (e.g. "enclave=;") to the default "/" rather than returning "". + if (end == position) { + return "/"; + } + 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; +} + +// 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; + ok = false; + if (submessage.kind != espp::RtpsParticipant::SubmessageKind::DATA || + (submessage.flags & kSubmessageFlagData) == 0) { + 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(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(view.writer_sn, little_endian)) { + return view; + } + + view.inline_qos_present = (submessage.flags & kSubmessageFlagInlineQos) != 0; + view.data_present = true; + + // 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; + } + + 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; + // 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; + } + 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) { + // 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; + 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 = config_.socket_log_level}); + metatraffic_unicast_receiver_ = + std::make_unique(UdpSocket::Config{.log_level = config_.socket_log_level}); + user_unicast_receiver_ = + 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"; + 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; + } + + 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(); + 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(); + } + { + std::lock_guard receivers_lock(receivers_mutex_); + 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(); + } +} + +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) { + // 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(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; +} + +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_; +} + +std::vector RtpsParticipant::writers() const { + std::lock_guard lock(mutex_); + return writers_; +} + +std::vector RtpsParticipant::readers() const { + std::lock_guard lock(mutex_); + 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)); + 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); + 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}, + next_spdp_sequence_number(), 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)); + 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); + 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}, + next_sedp_publication_sequence_number(), 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)); + 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); + 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}, + next_sedp_subscription_sequence_number(), payload) + .serialize(); +} + +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), cdr_payload) + .serialize(); +} + +bool RtpsParticipant::publish(std::string_view topic_name, std::span cdr_payload) { + 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_data_message(writer_config, cdr_payload); + + if (!user_unicast_receiver_) { + return false; + } + + 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 &send_config : send_configs) { + sent = user_unicast_receiver_->send(payload, send_config) || sent; + } + 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; +} + +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); + 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 = + 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; + } + + 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); + } + } + 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; + } + } + 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)) { + 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; + } + + 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; +} + +bool RtpsParticipant::handle_user_message(std::vector &data, const Socket::Info &sender) { + auto message = Message::parse(data); + if (!message) { + return false; + } + if (message->header.guid_prefix == guid_prefix_) { + 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; + } + + // 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_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(data_view.serialized_payload); + } + } + return false; +} + +bool RtpsParticipant::ensure_user_multicast_receivers_started(const std::string &extra_group) { + if (!started_.load()) { + return true; + } + + std::vector desired_groups; + 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_) { + add_group(reader_config.multicast_group); + } + } + + 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(), + [&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 = 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()); + 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; + } + 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 participants = discovered_participants(); + return std::accumulate(participants.begin(), participants.end(), send_spdp_announce_now(), + [this](bool sent, const auto &participant) { + return send_sedp_announcements_to(participant) || sent; + }); +} + +} // namespace espp 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 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..526875180 100644 --- a/doc/conf_common.py +++ b/doc/conf_common.py @@ -5,9 +5,15 @@ 'esp_docs.esp_extensions.dummy_build_system', 'esp_docs.esp_extensions.run_doxygen', 'myst_parser', + 'sphinxcontrib.mermaid', ] -exclude_paterns = ['build', '_build', 'detail'] +mermaid_output_format = 'raw' +mermaid_d3_zoom = True +mermaid_dark_theme = 'neutral' +mermaid_light_theme = 'neutral' + +exclude_patterns = ['build', '_build', 'detail'] # link roles config github_repo = 'esp-cpp/espp' 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. + * - 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. + * - 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..c54b698e6 100644 --- a/python/README.md +++ b/python/README.md @@ -47,6 +47,13 @@ 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 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 @@ -105,7 +112,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 +123,27 @@ 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 +``` + +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 +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..533b5d3c8 --- /dev/null +++ b/python/rtps_host.py @@ -0,0 +1,1202 @@ +#!/usr/bin/env python3 +"""Simple host-side RTPS test harness for the ESPP RTPS component. + +This script speaks the ESPP RTPS discovery wire format plus the standard +CDR-over-RTPS user-data path used by ``RtpsParticipant``. It is useful for: + +1. discovering an embedded ESPP RTPS participant from a PC/host, +2. inspecting SPDP/SEDP announcements, and +3. sending or receiving ``std_msgs/msg/UInt32``-style test samples. User samples + are standard RTPS ``DATA`` submessages whose serializedPayload is the raw + CDR-encapsulated sample; the topic is identified by the writer GUID resolved + through SEDP discovery (no ESPP-specific payload framing). + +Run ``python rtps_host.py --self-test`` to validate the wire-format encoders and +decoders against the firmware's expectations without any network I/O. + +It uses only the Python standard library, so it does not require Python +bindings or a rebuilt host ``lib/`` tree. +""" + +from __future__ import annotations + +import argparse +import hashlib +import ipaddress +import select +import socket +import struct +import sys +import time +from dataclasses import dataclass +from typing import Dict, Iterable, List, Optional, Set, Tuple + + +RTPS_MAGIC = b"RTPS" +PL_CDR_LE = b"\x00\x03\x00\x00" + +PORT_BASE = 7400 +DOMAIN_GAIN = 250 +PARTICIPANT_GAIN = 2 +METATRAFFIC_MULTICAST_OFFSET = 0 +METATRAFFIC_UNICAST_OFFSET = 10 +USER_MULTICAST_OFFSET = 1 +USER_UNICAST_OFFSET = 11 + +DATA_SUBMESSAGE_KIND = 0x15 +DATA_SUBMESSAGE_FLAGS = 0x01 | 0x04 +DATA_SUBMESSAGE_OCTETS_TO_INLINE_QOS = 16 + +RTPS_QOS_RELIABILITY_BEST_EFFORT = 1 +RTPS_QOS_RELIABILITY_RELIABLE = 2 + +KIND_UDP_V4 = 1 +VENDOR_ID = b"\xca\xfe" + +ENTITY_ID_UNKNOWN = b"\x00\x00\x00\x00" +PARTICIPANT_ENTITY_ID = b"\x00\x00\x01\xc1" +SPDP_WRITER_ENTITY_ID = b"\x00\x01\x00\xc2" +SPDP_READER_ENTITY_ID = b"\x00\x01\x00\xc7" +SEDP_PUBLICATIONS_WRITER_ENTITY_ID = b"\x00\x00\x03\xc2" +SEDP_PUBLICATIONS_READER_ENTITY_ID = b"\x00\x00\x03\xc7" +SEDP_SUBSCRIPTIONS_WRITER_ENTITY_ID = b"\x00\x00\x04\xc2" +SEDP_SUBSCRIPTIONS_READER_ENTITY_ID = b"\x00\x00\x04\xc7" +USER_WRITER_NO_KEY_KIND = 0x03 +USER_READER_NO_KEY_KIND = 0x04 + +BUILTIN_ENDPOINT_SET = ( + (1 << 0) + | (1 << 1) + | (1 << 2) + | (1 << 3) + | (1 << 4) + | (1 << 5) + | (1 << 10) + | (1 << 11) +) + +PID_SENTINEL = 0x0001 +PID_PARTICIPANT_LEASE_DURATION = 0x0002 +PID_TOPIC_NAME = 0x0005 +PID_TYPE_NAME = 0x0007 +PID_DOMAIN_ID = 0x000F +PID_PROTOCOL_VERSION = 0x0015 +PID_VENDORID = 0x0016 +PID_RELIABILITY = 0x001A +PID_LIVELINESS = 0x001B +PID_DURABILITY = 0x001D +PID_USER_DATA = 0x002C +PID_MULTICAST_LOCATOR = 0x0030 +PID_UNICAST_LOCATOR = 0x002F +PID_DEFAULT_UNICAST_LOCATOR = 0x0031 +PID_METATRAFFIC_UNICAST_LOCATOR = 0x0032 +PID_METATRAFFIC_MULTICAST_LOCATOR = 0x0033 +PID_HISTORY = 0x0040 +PID_EXPECTS_INLINE_QOS = 0x0043 +PID_DEFAULT_MULTICAST_LOCATOR = 0x0048 +PID_PARTICIPANT_GUID = 0x0050 +PID_BUILTIN_ENDPOINT_SET = 0x0058 +PID_ENDPOINT_GUID = 0x005A +PID_TYPE_MAX_SIZE_SERIALIZED = 0x0060 +PID_ENTITY_NAME = 0x0062 +PID_KEY_HASH = 0x0070 + +DEFAULT_LEASE_DURATION_SECONDS = 20 +DEFAULT_LEASE_DURATION_NANOSECONDS = 0 +DEFAULT_MAX_BLOCKING_SECONDS = 0 +DEFAULT_MAX_BLOCKING_NANOSECONDS = 100_000_000 +DEFAULT_TOPIC_PREFIX = "espp/rtps_example" +DEFAULT_REQUEST_TOPIC = f"{DEFAULT_TOPIC_PREFIX}/request" +DEFAULT_RESPONSE_TOPIC = f"{DEFAULT_TOPIC_PREFIX}/response" + + +@dataclass +class PortMapping: + metatraffic_multicast: int + metatraffic_unicast: int + user_multicast: int + user_unicast: int + + +@dataclass +class ParticipantProxy: + participant_guid: bytes + guid_prefix: bytes + name: str + enclave: str + address: str + ports: PortMapping + builtin_endpoints: int + + +@dataclass +class EndpointProxy: + guid: bytes + participant_guid: bytes + topic_name: str + type_name: str + reliability: str + is_reader: bool + expects_inline_qos: bool + unicast_address: str + unicast_port: int + multicast_locators: List[Tuple[str, int]] + + +@dataclass +class WriterConfig: + topic_name: str + type_name: str + reliable: bool + entity_index: int + + +@dataclass +class ReaderConfig: + topic_name: str + type_name: str + reliable: bool + entity_index: int + + +def log(message: str) -> None: + print(message, flush=True) + + +def hex_string(value: bytes) -> str: + return value.hex() + + +def guid_to_string(guid: bytes) -> str: + return hex_string(guid[:12]) + ":" + hex_string(guid[12:]) + + +def entity_id_for_index(entity_index: int, kind: int) -> bytes: + return bytes((0x00, 0x00, 0x10 + entity_index, kind)) + + +def reliability_to_name(reliable: bool) -> str: + return "reliable" if reliable else "best-effort" + + +def ntp_fraction_from_nanoseconds(nanoseconds: int) -> int: + # RTPS Duration_t/Time_t use NTP fraction units of 1/2^32 s, not nanoseconds. + return (nanoseconds << 32) // 1_000_000_000 + + +def padded_parameter_length(length: int) -> int: + # RTPS PL_CDR requires each parameterLength to be a multiple of 4. + return (length + 3) & ~3 + + +def compute_port_mapping(domain_id: int, participant_id: int) -> PortMapping: + base = PORT_BASE + DOMAIN_GAIN * domain_id + participant_offset = PARTICIPANT_GAIN * participant_id + return PortMapping( + metatraffic_multicast=base + METATRAFFIC_MULTICAST_OFFSET, + metatraffic_unicast=base + METATRAFFIC_UNICAST_OFFSET + participant_offset, + user_multicast=base + USER_MULTICAST_OFFSET, + user_unicast=base + USER_UNICAST_OFFSET + participant_offset, + ) + + +def guess_local_ipv4() -> str: + probe = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) + try: + probe.connect(("8.8.8.8", 80)) + return probe.getsockname()[0] + except OSError: + return "127.0.0.1" + finally: + probe.close() + + +def make_guid_prefix(node_name: str, domain_id: int, participant_id: int) -> bytes: + digest = hashlib.sha256(node_name.encode("utf-8")).digest() + return bytes( + ( + participant_id & 0xFF, + (participant_id >> 8) & 0xFF, + domain_id & 0xFF, + (domain_id >> 8) & 0xFF, + ) + ) + digest[:8] + + +def make_guid(prefix: bytes, entity_id: bytes) -> bytes: + return prefix + entity_id + + +def align4(buffer: bytearray) -> None: + while len(buffer) % 4 != 0: + buffer.append(0) + + +def append_parameter_header(buffer: bytearray, pid: int, length: int) -> None: + buffer.extend(struct.pack(" None: + append_parameter_header(buffer, pid, 16) + buffer.extend(guid) + + +def append_parameter_protocol_version(buffer: bytearray) -> None: + append_parameter_header(buffer, PID_PROTOCOL_VERSION, 4) + buffer.extend((2, 3, 0, 0)) + + +def append_parameter_vendor_id(buffer: bytearray) -> None: + append_parameter_header(buffer, PID_VENDORID, 4) + buffer.extend(VENDOR_ID) + buffer.extend((0, 0)) + + +def append_parameter_u32(buffer: bytearray, pid: int, value: int) -> None: + append_parameter_header(buffer, pid, 4) + buffer.extend(struct.pack(" None: + append_parameter_header(buffer, pid, 4) + buffer.extend((1 if value else 0, 0, 0, 0)) + + +def append_parameter_duration(buffer: bytearray, pid: int, seconds: int, nanoseconds: int) -> None: + append_parameter_header(buffer, pid, 8) + buffer.extend(struct.pack(" bytes: + # Locator_t.kind/.port are little-endian in PL_CDR_LE; only the 16-byte address is raw bytes. + locator = bytearray(24) + struct.pack_into(" None: + append_parameter_header(buffer, pid, 24) + buffer.extend(locator_bytes(ip_address, port)) + + +def append_parameter_string_cdr(buffer: bytearray, pid: int, text: str) -> None: + encoded = text.encode("utf-8") + # parameterLength must be a multiple of 4 and includes the trailing CDR padding. + append_parameter_header(buffer, pid, padded_parameter_length(4 + len(encoded) + 1)) + buffer.extend(struct.pack(" None: + append_parameter_header(buffer, pid, padded_parameter_length(4 + len(payload))) + buffer.extend(struct.pack(" None: + append_parameter_header(buffer, PID_RELIABILITY, 12) + kind = RTPS_QOS_RELIABILITY_RELIABLE if reliable else RTPS_QOS_RELIABILITY_BEST_EFFORT + buffer.extend(struct.pack(" None: + append_parameter_header(buffer, PID_DURABILITY, 4) + buffer.extend(struct.pack(" None: + append_parameter_header(buffer, PID_LIVELINESS, 12) + buffer.extend(struct.pack(" None: + append_parameter_header(buffer, PID_HISTORY, 8) + buffer.extend(struct.pack(" None: + append_parameter_header(buffer, PID_KEY_HASH, 16) + buffer.extend(guid) + + +def append_parameter_sentinel(buffer: bytearray) -> None: + append_parameter_header(buffer, PID_SENTINEL, 0) + + +def build_parameter_list_payload(parameter_buffer: bytearray) -> bytes: + return PL_CDR_LE + bytes(parameter_buffer) + + +def build_data_submessage(reader_id: bytes, writer_id: bytes, sequence_number: int, payload: bytes) -> bytes: + high = sequence_number >> 32 + low = sequence_number & 0xFFFFFFFF + submessage_payload = bytearray() + submessage_payload.extend(struct.pack(" bytes: + header = RTPS_MAGIC + bytes((2, 3)) + VENDOR_ID + guid_prefix + return header + build_data_submessage(reader_id, writer_id, sequence_number, payload) + + +def parse_parameter_list(payload: bytes) -> List[tuple[int, bytes]]: + if len(payload) < 4 or payload[:4] != PL_CDR_LE: + return [] + parameters: List[tuple[int, bytes]] = [] + offset = 4 + while offset + 4 <= len(payload): + pid, length = struct.unpack_from(" len(payload): + return [] + value = payload[offset : offset + length] + parameters.append((pid, value)) + offset += length + offset += (4 - (length % 4)) & 0x3 + return parameters + + +def find_parameter(parameters: Iterable[tuple[int, bytes]], pid: int) -> Optional[bytes]: + for candidate_pid, candidate_value in parameters: + if candidate_pid == pid: + return candidate_value + return None + + +def find_parameters(parameters: Iterable[tuple[int, bytes]], pid: int) -> List[bytes]: + return [candidate_value for candidate_pid, candidate_value in parameters if candidate_pid == pid] + + +def parse_guid(value: Optional[bytes]) -> Optional[bytes]: + if value is None or len(value) != 16: + return None + return value + + +def parse_u32_le(value: Optional[bytes]) -> Optional[int]: + if value is None or len(value) < 4: + return None + return struct.unpack_from(" Optional[bool]: + if value is None or not value: + return None + return value[0] != 0 + + +def parse_cdr_string(value: Optional[bytes]) -> Optional[str]: + if value is None or len(value) < 4: + return None + length = struct.unpack_from(" len(value): + return None + raw = value[4 : 4 + length] + if raw.endswith(b"\x00"): + raw = raw[:-1] + return raw.decode("utf-8", errors="replace") + + +def parse_octet_sequence(value: Optional[bytes]) -> Optional[bytes]: + if value is None or len(value) < 4: + return None + length = struct.unpack_from(" len(value): + return None + return value[4 : 4 + length] + + +def parse_locator(value: Optional[bytes]) -> tuple[str, int]: + if value is None or len(value) != 24: + return ("0.0.0.0", 0) + kind = struct.unpack_from(" str: + kind = parse_u32_le(value) + return "reliable" if kind == RTPS_QOS_RELIABILITY_RELIABLE else "best-effort" + + +def extract_enclave(value: Optional[bytes]) -> str: + if not value: + return "/" + text = value.decode("utf-8", errors="replace") + marker = "enclave=" + start = text.find(marker) + if start < 0: + return "/" + start += len(marker) + end = text.find(";", start) + if end < 0: + end = len(text) + return text[start:end] or "/" + + +def serialize_uint32_cdr(value: int) -> bytes: + return b"\x00\x01\x00\x00" + struct.pack(" Optional[int]: + if len(payload) < 8 or payload[:2] != b"\x00\x01": + return None + return struct.unpack_from(" None: + self.args = args + self.ports = compute_port_mapping(args.domain_id, args.participant_id) + self.guid_prefix = make_guid_prefix(args.node_name, args.domain_id, args.participant_id) + self.participant_guid = make_guid(self.guid_prefix, PARTICIPANT_ENTITY_ID) + self.sequence_numbers: Dict[bytes, int] = {} + self.discovered_participants: Dict[bytes, ParticipantProxy] = {} + self.discovered_writers: Dict[bytes, EndpointProxy] = {} + self.discovered_readers: Dict[bytes, EndpointProxy] = {} + self.joined_user_multicast_groups: Set[str] = set() + + self.local_writers = [ + WriterConfig( + topic_name=args.publish_topic, + type_name=args.type_name, + reliable=args.reliable, + entity_index=0, + ) + ] if args.publish_topic else [] + self.local_readers = [ + ReaderConfig( + topic_name=topic_name, + type_name=args.type_name, + reliable=False, + entity_index=index, + ) + for index, topic_name in enumerate(args.subscribe_topic) + ] + + self.metatraffic_multicast_sock = self._create_metatraffic_multicast_socket() + self.metatraffic_unicast_sock = self._create_bound_udp_socket(self.ports.metatraffic_unicast) + self.user_unicast_sock = self._create_bound_udp_socket(self.ports.user_unicast) + self.user_multicast_sock = self._create_user_multicast_socket() + self._configure_multicast_sender(self.metatraffic_unicast_sock) + self._configure_multicast_sender(self.user_unicast_sock) + + self.next_discovery_send = 0.0 + self.next_publish_send = 0.0 + self.last_no_participant_log = 0.0 + self.last_unknown_writer_log = 0.0 + + def _create_bound_udp_socket(self, port: int) -> socket.socket: + sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM, socket.IPPROTO_UDP) + sock.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1) + if hasattr(socket, "SO_REUSEPORT"): + try: + sock.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEPORT, 1) + except OSError: + # Some platforms expose SO_REUSEPORT but reject setting it; this is + # a best-effort optimization and is not required for correctness. + pass + sock.bind((self.args.bind_address, port)) + sock.setblocking(False) + return sock + + def _create_metatraffic_multicast_socket(self) -> socket.socket: + sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM, socket.IPPROTO_UDP) + sock.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1) + if hasattr(socket, "SO_REUSEPORT"): + try: + sock.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEPORT, 1) + except OSError: + # Some platforms expose SO_REUSEPORT but reject setting it; this is + # a best-effort optimization and is not required for correctness. + pass + try: + sock.bind((self.args.multicast_group, self.ports.metatraffic_multicast)) + except OSError: + # Not all platforms allow binding directly to the multicast group + # address, so fall back to the selected local interface address. + sock.bind((self.args.bind_address, self.ports.metatraffic_multicast)) + interface_ip = self.args.multicast_interface or self.args.advertised_address + membership = socket.inet_aton(self.args.multicast_group) + socket.inet_aton(interface_ip) + sock.setsockopt(socket.IPPROTO_IP, socket.IP_ADD_MEMBERSHIP, membership) + sock.setblocking(False) + return sock + + def _create_user_multicast_socket(self) -> socket.socket: + sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM, socket.IPPROTO_UDP) + sock.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1) + if hasattr(socket, "SO_REUSEPORT"): + try: + sock.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEPORT, 1) + except OSError: + # Some platforms expose SO_REUSEPORT but reject setting it; this is + # a best-effort optimization and is not required for correctness. + pass + # Bind to INADDR_ANY, not a unicast address: multicast datagrams are addressed to the group + # (e.g. 239.255.0.11), so a socket bound to a specific unicast interface address will not + # receive them on Linux (and unreliably on macOS). Delivery is decided by the joined + # group(s) + port. This socket may join several user-data groups, so binding to a single + # group address is not an option. + sock.bind(("", self.ports.user_multicast)) + sock.setblocking(False) + return sock + + def _join_user_multicast_group(self, group: str) -> None: + if group in self.joined_user_multicast_groups: + return + interface_ip = self.args.multicast_interface or self.args.advertised_address + membership = socket.inet_aton(group) + socket.inet_aton(interface_ip) + self.user_multicast_sock.setsockopt(socket.IPPROTO_IP, socket.IP_ADD_MEMBERSHIP, membership) + self.joined_user_multicast_groups.add(group) + log(f"[multicast] joined user-data group {group}:{self.ports.user_multicast}") + + def _configure_multicast_sender(self, sock: socket.socket) -> None: + sock.setsockopt(socket.IPPROTO_IP, socket.IP_MULTICAST_TTL, 1) + sock.setsockopt(socket.IPPROTO_IP, socket.IP_MULTICAST_LOOP, 1) + interface_ip = self.args.multicast_interface or self.args.advertised_address + sock.setsockopt(socket.IPPROTO_IP, socket.IP_MULTICAST_IF, socket.inet_aton(interface_ip)) + + def _next_sequence(self, writer_entity_id: bytes) -> int: + value = self.sequence_numbers.get(writer_entity_id, 1) + self.sequence_numbers[writer_entity_id] = value + 1 + return value + + def _local_writer_guid(self, entity_index: int) -> bytes: + return make_guid(self.guid_prefix, entity_id_for_index(entity_index, USER_WRITER_NO_KEY_KIND)) + + def _local_reader_guid(self, entity_index: int) -> bytes: + return make_guid(self.guid_prefix, entity_id_for_index(entity_index, USER_READER_NO_KEY_KIND)) + + def build_spdp_announce_message(self) -> bytes: + parameters = bytearray() + append_parameter_protocol_version(parameters) + append_parameter_vendor_id(parameters) + append_parameter_u32(parameters, PID_DOMAIN_ID, self.args.domain_id) + append_parameter_guid(parameters, PID_PARTICIPANT_GUID, self.participant_guid) + append_parameter_locator( + parameters, + PID_METATRAFFIC_MULTICAST_LOCATOR, + self.args.multicast_group, + self.ports.metatraffic_multicast, + ) + append_parameter_locator( + parameters, + PID_METATRAFFIC_UNICAST_LOCATOR, + self.args.advertised_address, + self.ports.metatraffic_unicast, + ) + append_parameter_locator( + parameters, + PID_DEFAULT_UNICAST_LOCATOR, + self.args.advertised_address, + self.ports.user_unicast, + ) + append_parameter_locator( + parameters, + PID_DEFAULT_MULTICAST_LOCATOR, + self.args.multicast_group, + self.ports.user_multicast, + ) + append_parameter_duration( + parameters, + PID_PARTICIPANT_LEASE_DURATION, + DEFAULT_LEASE_DURATION_SECONDS, + DEFAULT_LEASE_DURATION_NANOSECONDS, + ) + append_parameter_u32(parameters, PID_BUILTIN_ENDPOINT_SET, BUILTIN_ENDPOINT_SET) + append_parameter_octet_sequence( + parameters, + PID_USER_DATA, + f"enclave={self.args.enclave};".encode("utf-8"), + ) + append_parameter_string_cdr(parameters, PID_ENTITY_NAME, self.args.node_name) + append_parameter_sentinel(parameters) + payload = build_parameter_list_payload(parameters) + return build_rtps_message( + self.guid_prefix, + ENTITY_ID_UNKNOWN, + SPDP_WRITER_ENTITY_ID, + self._next_sequence(SPDP_WRITER_ENTITY_ID), + payload, + ) + + def build_sedp_publication_message(self, writer: WriterConfig) -> bytes: + guid = self._local_writer_guid(writer.entity_index) + parameters = bytearray() + append_parameter_guid(parameters, PID_ENDPOINT_GUID, guid) + append_parameter_locator(parameters, PID_UNICAST_LOCATOR, self.args.advertised_address, self.ports.user_unicast) + append_parameter_guid(parameters, PID_PARTICIPANT_GUID, self.participant_guid) + append_parameter_string_cdr(parameters, PID_TOPIC_NAME, writer.topic_name) + append_parameter_string_cdr(parameters, PID_TYPE_NAME, writer.type_name) + append_parameter_key_hash(parameters, guid) + append_parameter_u32(parameters, PID_TYPE_MAX_SIZE_SERIALIZED, 8) + append_parameter_protocol_version(parameters) + append_parameter_vendor_id(parameters) + append_parameter_durability(parameters) + append_parameter_liveliness(parameters) + append_parameter_reliability(parameters, writer.reliable) + append_parameter_history(parameters) + append_parameter_sentinel(parameters) + payload = build_parameter_list_payload(parameters) + return build_rtps_message( + self.guid_prefix, + SEDP_PUBLICATIONS_READER_ENTITY_ID, + SEDP_PUBLICATIONS_WRITER_ENTITY_ID, + self._next_sequence(SEDP_PUBLICATIONS_WRITER_ENTITY_ID), + payload, + ) + + def build_sedp_subscription_message(self, reader: ReaderConfig) -> bytes: + guid = self._local_reader_guid(reader.entity_index) + parameters = bytearray() + append_parameter_guid(parameters, PID_ENDPOINT_GUID, guid) + append_parameter_locator(parameters, PID_UNICAST_LOCATOR, self.args.advertised_address, self.ports.user_unicast) + append_parameter_bool(parameters, PID_EXPECTS_INLINE_QOS, False) + append_parameter_guid(parameters, PID_PARTICIPANT_GUID, self.participant_guid) + append_parameter_string_cdr(parameters, PID_TOPIC_NAME, reader.topic_name) + append_parameter_string_cdr(parameters, PID_TYPE_NAME, reader.type_name) + append_parameter_key_hash(parameters, guid) + append_parameter_protocol_version(parameters) + append_parameter_vendor_id(parameters) + append_parameter_durability(parameters) + append_parameter_liveliness(parameters) + append_parameter_reliability(parameters, reader.reliable) + append_parameter_history(parameters) + append_parameter_sentinel(parameters) + payload = build_parameter_list_payload(parameters) + return build_rtps_message( + self.guid_prefix, + SEDP_SUBSCRIPTIONS_READER_ENTITY_ID, + SEDP_SUBSCRIPTIONS_WRITER_ENTITY_ID, + self._next_sequence(SEDP_SUBSCRIPTIONS_WRITER_ENTITY_ID), + payload, + ) + + def build_data_message(self, writer: WriterConfig, cdr_payload: bytes) -> bytes: + # Standard RTPS: the DATA serializedPayload is exactly the CDR-encapsulated sample. + writer_entity_id = entity_id_for_index(writer.entity_index, USER_WRITER_NO_KEY_KIND) + return build_rtps_message( + self.guid_prefix, + ENTITY_ID_UNKNOWN, + writer_entity_id, + self._next_sequence(writer_entity_id), + cdr_payload, + ) + + def send_spdp_announce_now(self) -> None: + payload = self.build_spdp_announce_message() + self.metatraffic_unicast_sock.sendto( + payload, + (self.args.multicast_group, self.ports.metatraffic_multicast), + ) + + def send_sedp_announcements_to(self, participant: ParticipantProxy) -> None: + target = (participant.address, participant.ports.metatraffic_unicast) + if participant.ports.metatraffic_unicast == 0 or not participant.address: + return + for writer in self.local_writers: + self.metatraffic_unicast_sock.sendto(self.build_sedp_publication_message(writer), target) + for reader in self.local_readers: + self.metatraffic_unicast_sock.sendto(self.build_sedp_subscription_message(reader), target) + + def send_discovery_now(self) -> None: + self.send_spdp_announce_now() + for participant in list(self.discovered_participants.values()): + self.send_sedp_announcements_to(participant) + + def publish_now(self) -> None: + if not self.local_writers: + return + if not self._publish_value(self.local_writers[0], self.args.publish_value): + now = time.monotonic() + if now - self.last_no_participant_log > 2.0: + log( + f"[publish] no discovered participants yet for topic '{self.local_writers[0].topic_name}', " + "waiting for SPDP" + ) + self.last_no_participant_log = now + else: + log( + f"[publish] sent {self.args.publish_value} on '{self.local_writers[0].topic_name}' " + f"using {len(self._build_user_targets(self.local_writers[0]))} discovered target(s)" + ) + + def _build_user_targets(self, writer: WriterConfig) -> List[Tuple[str, int]]: + targets: List[Tuple[str, int]] = [] + for reader in self.discovered_readers.values(): + if reader.topic_name != writer.topic_name: + continue + if reader.multicast_locators: + for multicast_address, multicast_port in reader.multicast_locators: + target = (multicast_address, multicast_port) + if target not in targets: + targets.append(target) + continue + if reader.unicast_port > 0 and reader.unicast_address: + target = (reader.unicast_address, reader.unicast_port) + if target not in targets: + targets.append(target) + if targets: + return targets + for participant in self.discovered_participants.values(): + target = (participant.address, participant.ports.user_unicast) + if participant.address and participant.ports.user_unicast > 0 and target not in targets: + targets.append(target) + return targets + + def _publish_value(self, writer: WriterConfig, value: int, target: Optional[tuple[str, int]] = None) -> bool: + payload = self.build_data_message(writer, serialize_uint32_cdr(value)) + if target is not None: + self.user_unicast_sock.sendto(payload, target) + return True + targets = self._build_user_targets(writer) + if not targets: + return False + for destination in targets: + self.user_unicast_sock.sendto(payload, destination) + return True + + def handle_metatraffic_packet(self, packet: bytes, sender_ip: str) -> None: + for _guid_prefix, writer_id, serialized_payload in parse_rtps_data_messages(packet): + parameters = parse_parameter_list(serialized_payload) + if not parameters: + continue + + if writer_id == SPDP_WRITER_ENTITY_ID: + self._handle_spdp(parameters, sender_ip) + elif writer_id == SEDP_PUBLICATIONS_WRITER_ENTITY_ID: + self._handle_sedp(parameters, sender_ip, is_reader=False) + elif writer_id == SEDP_SUBSCRIPTIONS_WRITER_ENTITY_ID: + self._handle_sedp(parameters, sender_ip, is_reader=True) + + def _handle_spdp(self, parameters: List[tuple[int, bytes]], sender_ip: str) -> None: + participant_guid = parse_guid(find_parameter(parameters, PID_PARTICIPANT_GUID)) + if participant_guid is None or participant_guid[:12] == self.guid_prefix: + return + + meta_ip, meta_uc_port = parse_locator(find_parameter(parameters, PID_METATRAFFIC_UNICAST_LOCATOR)) + _, meta_mc_port = parse_locator(find_parameter(parameters, PID_METATRAFFIC_MULTICAST_LOCATOR)) + user_ip, user_uc_port = parse_locator(find_parameter(parameters, PID_DEFAULT_UNICAST_LOCATOR)) + _, user_mc_port = parse_locator(find_parameter(parameters, PID_DEFAULT_MULTICAST_LOCATOR)) + + participant = ParticipantProxy( + participant_guid=participant_guid, + guid_prefix=participant_guid[:12], + name=parse_cdr_string(find_parameter(parameters, PID_ENTITY_NAME)) or "", + enclave=extract_enclave(parse_octet_sequence(find_parameter(parameters, PID_USER_DATA))), + address=user_ip if user_ip != "0.0.0.0" else sender_ip, + ports=PortMapping( + metatraffic_multicast=meta_mc_port, + metatraffic_unicast=meta_uc_port, + user_multicast=user_mc_port, + user_unicast=user_uc_port, + ), + builtin_endpoints=parse_u32_le(find_parameter(parameters, PID_BUILTIN_ENDPOINT_SET)) or 0, + ) + + is_new = participant_guid not in self.discovered_participants + self.discovered_participants[participant_guid] = participant + label = participant.name or hex_string(participant.guid_prefix) + log( + f"[spdp] participant '{label}' at {participant.address} " + f"(meta={participant.ports.metatraffic_unicast}, user={participant.ports.user_unicast}, " + f"enclave={participant.enclave})" + ) + if is_new: + self.send_sedp_announcements_to(participant) + + def _handle_sedp(self, parameters: List[tuple[int, bytes]], sender_ip: str, is_reader: bool) -> None: + endpoint_guid = parse_guid(find_parameter(parameters, PID_ENDPOINT_GUID)) + if endpoint_guid is None or endpoint_guid[:12] == self.guid_prefix: + return + + participant_guid = parse_guid(find_parameter(parameters, PID_PARTICIPANT_GUID)) + if participant_guid is None: + participant_guid = endpoint_guid[:12] + PARTICIPANT_ENTITY_ID + + endpoint_ip, endpoint_port = parse_locator(find_parameter(parameters, PID_UNICAST_LOCATOR)) + multicast_locators = [ + parse_locator(value) + for value in find_parameters(parameters, PID_MULTICAST_LOCATOR) + ] + multicast_locators = [ + (multicast_address, multicast_port) + for multicast_address, multicast_port in multicast_locators + if multicast_address != "0.0.0.0" and multicast_port > 0 + ] + endpoint = EndpointProxy( + guid=endpoint_guid, + participant_guid=participant_guid, + topic_name=parse_cdr_string(find_parameter(parameters, PID_TOPIC_NAME)) or "", + type_name=parse_cdr_string(find_parameter(parameters, PID_TYPE_NAME)) or "", + reliability=parse_reliability(find_parameter(parameters, PID_RELIABILITY)), + is_reader=is_reader, + expects_inline_qos=parse_bool(find_parameter(parameters, PID_EXPECTS_INLINE_QOS)) or False, + unicast_address=endpoint_ip if endpoint_ip != "0.0.0.0" else sender_ip, + unicast_port=endpoint_port, + multicast_locators=multicast_locators, + ) + endpoint_map = self.discovered_readers if is_reader else self.discovered_writers + is_new = endpoint_guid not in endpoint_map + endpoint_map[endpoint_guid] = endpoint + if not is_reader: + subscribed_topics = {reader.topic_name for reader in self.local_readers} + if endpoint.topic_name in subscribed_topics: + for multicast_address, multicast_port in endpoint.multicast_locators: + if multicast_port == self.ports.user_multicast: + self._join_user_multicast_group(multicast_address) + if is_new: + kind = "reader" if is_reader else "writer" + log( + f"[sedp] {kind} topic='{endpoint.topic_name}' type='{endpoint.type_name}' " + f"reliability={endpoint.reliability} participant={guid_to_string(endpoint.participant_guid)}" + ) + + def handle_user_packet(self, packet: bytes, sender_ip: str, sender_port: int) -> None: + subscribed_topics = {reader.topic_name for reader in self.local_readers} + for guid_prefix, writer_id, serialized_payload in parse_rtps_data_messages(packet): + # Standard RTPS: resolve the topic from the writer GUID via SEDP discovery state. + writer_guid = guid_prefix + writer_id + writer = self.discovered_writers.get(writer_guid) + if writer is None: + # Sample arrived before its writer was discovered via SEDP; drop it (best-effort). + # Surface it (rate-limited) so a missing SEDP exchange is visible rather than silent. + now = time.monotonic() + if now - self.last_unknown_writer_log > 2.0: + log( + f"[data] received {len(serialized_payload)}-byte sample from UNDISCOVERED " + f"writer {guid_to_string(writer_guid)} at {sender_ip}:{sender_port}; cannot " + f"route without SEDP (discovered_writers={len(self.discovered_writers)})" + ) + self.last_unknown_writer_log = now + continue + topic_name = writer.topic_name + if topic_name not in subscribed_topics: + continue + maybe_value = deserialize_uint32_cdr(serialized_payload) + if maybe_value is None: + continue + log( + f"[data] topic='{topic_name}' value={maybe_value} reliability={writer.reliability} " + f"from {sender_ip}:{sender_port} writer={hex_string(writer_id)}" + ) + if self.args.echo_received and self.local_writers: + out_writer = self.local_writers[0] + if self._publish_value(out_writer, maybe_value): + log(f"[echo] responded with value={maybe_value} on '{out_writer.topic_name}'") + else: + self._publish_value(out_writer, maybe_value, (sender_ip, sender_port)) + log( + f"[echo] responded with value={maybe_value} on '{out_writer.topic_name}' " + f"to {sender_ip}:{sender_port}" + ) + + def run(self) -> None: + start_time = time.monotonic() + self.next_discovery_send = start_time + self.next_publish_send = start_time + self.args.publish_interval + + log( + "Starting RTPS host harness\n" + f" node: {self.args.node_name}\n" + f" advertised address: {self.args.advertised_address}\n" + f" domain/participant: {self.args.domain_id}/{self.args.participant_id}\n" + f" ports: meta_mc={self.ports.metatraffic_multicast}, meta_uc={self.ports.metatraffic_unicast}, " + f"user_mc={self.ports.user_multicast}, user_uc={self.ports.user_unicast}" + ) + if self.local_readers: + log(" readers: " + ", ".join(reader.topic_name for reader in self.local_readers)) + if self.local_writers: + writer = self.local_writers[0] + writer_mode = "echo responder" if self.args.echo_received else "periodic publisher" + interval_text = ( + f", publish value={self.args.publish_value} every {self.args.publish_interval:.2f}s" + if self.args.publish_interval > 0 + else "" + ) + log(f" writer: {writer.topic_name} ({reliability_to_name(writer.reliable)}, {writer_mode}{interval_text})") + + try: + while True: + now = time.monotonic() + if now >= self.next_discovery_send: + self.send_discovery_now() + self.next_discovery_send = now + self.args.announce_period + if self.local_writers and self.args.publish_interval > 0 and now >= self.next_publish_send: + self.publish_now() + self.next_publish_send = now + self.args.publish_interval + if self.args.duration > 0 and now - start_time >= self.args.duration: + break + + readable, _, _ = select.select( + [ + self.metatraffic_multicast_sock, + self.metatraffic_unicast_sock, + self.user_unicast_sock, + self.user_multicast_sock, + ], + [], + [], + 0.2, + ) + for sock in readable: + packet, sender = sock.recvfrom(4096) + sender_ip, sender_port = sender[0], sender[1] + if sock is self.user_unicast_sock or sock is self.user_multicast_sock: + self.handle_user_packet(packet, sender_ip, sender_port) + else: + self.handle_metatraffic_packet(packet, sender_ip) + except KeyboardInterrupt: + log("Stopping RTPS host harness") + finally: + self.close() + + def close(self) -> None: + for sock in ( + self.metatraffic_multicast_sock, + self.metatraffic_unicast_sock, + self.user_unicast_sock, + self.user_multicast_sock, + ): + try: + sock.close() + except OSError as exc: + log(f"[close] ignoring socket close failure for {sock!r}: {exc}") + + +def parse_rtps_data_messages(packet: bytes) -> List[tuple[bytes, bytes, bytes]]: + """Return (guid_prefix, writer_id, serialized_payload) for each DATA submessage.""" + if len(packet) < 20 or not packet.startswith(RTPS_MAGIC): + return [] + guid_prefix = packet[8:20] + offset = 20 + messages: List[tuple[bytes, bytes, bytes]] = [] + while offset + 4 <= len(packet): + kind = packet[offset] + flags = packet[offset + 1] + length = struct.unpack_from(" len(packet): + break + payload = packet[offset : offset + length] + offset += length + if kind != DATA_SUBMESSAGE_KIND or (flags & 0x04) == 0: + continue + if len(payload) < 20: + continue + extra_flags, octets_to_inline_qos = struct.unpack_from(" int: + """Validate the wire-format encoders/decoders against firmware expectations (no network I/O).""" + failures: List[str] = [] + + def check(name: str, condition: bool) -> None: + log(f" [{'PASS' if condition else 'FAIL'}] {name}") + if not condition: + failures.append(name) + + # Locators: kind/port are little-endian, address is raw network-order bytes; round-trips. + loc = locator_bytes("192.168.1.5", 7411) + check("locator kind is little-endian", loc[:4] == struct.pack(" 16 + append_parameter_string_cdr(params, PID_TYPE_NAME, "std_msgs::msg::dds_::UInt32_") + append_parameter_octet_sequence(params, PID_USER_DATA, b"enclave=/;") # body 4+10=14 -> 16 + offset = 0 + aligned = True + while offset + 4 <= len(params): + pid, length = struct.unpack_from(" argparse.Namespace: + parser = argparse.ArgumentParser( + description=( + "Discover an ESPP RTPS participant from a PC/host and optionally " + "exchange temporary UInt32 user-data samples." + ) + ) + parser.add_argument("--node-name", default="python_rtps_host", help="Local participant name") + parser.add_argument("--domain-id", type=int, default=0, help="RTPS domain id") + parser.add_argument("--participant-id", type=int, default=10, help="Local participant id") + parser.add_argument( + "--bind-address", + default=None, + help="Local bind address (defaults to the advertised address rather than all interfaces)", + ) + parser.add_argument( + "--advertised-address", + default=None, + help="IPv4 address to advertise to peers (defaults to best-effort local IPv4)", + ) + parser.add_argument( + "--multicast-interface", + default=None, + help="IPv4 interface to use for multicast join/send (defaults to advertised address)", + ) + parser.add_argument("--multicast-group", default="239.255.0.1", help="RTPS metatraffic multicast group") + parser.add_argument("--enclave", default="/", help="Enclave string advertised in SPDP user data") + parser.add_argument( + "--subscribe-topic", + action="append", + default=None, + help=f"Topic name to advertise as a local reader (repeatable). Defaults to {DEFAULT_REQUEST_TOPIC}.", + ) + parser.add_argument( + "--publish-topic", + default=None, + help=f"Topic name to publish as a local writer. Defaults to {DEFAULT_RESPONSE_TOPIC}.", + ) + parser.add_argument("--publish-value", type=int, default=42, help="UInt32 value to publish") + parser.add_argument( + "--publish-interval", + type=float, + default=0.0, + help="Seconds between periodic publish attempts when --publish-topic is set (0 disables periodic publishing)", + ) + parser.set_defaults(echo_received=True) + parser.add_argument( + "--echo-received", + dest="echo_received", + action="store_true", + help="Echo received subscribed-topic values back on the publish topic (enabled by default)", + ) + parser.add_argument( + "--no-echo-received", + dest="echo_received", + action="store_false", + help="Disable request/response echo behavior and only use periodic publishing", + ) + parser.add_argument("--reliable", action="store_true", help="Mark the local writer as reliable") + parser.add_argument("--type-name", default="std_msgs/msg/UInt32", help="Advertised type name") + parser.add_argument( + "--announce-period", + type=float, + default=1.0, + help="Seconds between periodic SPDP/SEDP discovery announcements", + ) + parser.add_argument( + "--duration", + type=float, + default=0.0, + help="Stop after this many seconds (0 = run until Ctrl+C)", + ) + args = parser.parse_args() + + if args.subscribe_topic is None: + args.subscribe_topic = [DEFAULT_REQUEST_TOPIC] + if args.publish_topic is None: + args.publish_topic = DEFAULT_RESPONSE_TOPIC + if args.advertised_address is None: + args.advertised_address = guess_local_ipv4() + if args.bind_address is None: + args.bind_address = args.advertised_address + try: + ipaddress.IPv4Address(args.bind_address) + ipaddress.IPv4Address(args.advertised_address) + ipaddress.IPv4Address(args.multicast_interface or args.advertised_address) + ipaddress.IPv4Address(args.multicast_group) + except ipaddress.AddressValueError as exc: + parser.error(str(exc)) + if args.publish_interval < 0: + parser.error("--publish-interval must be >= 0") + if args.announce_period <= 0: + parser.error("--announce-period must be > 0") + return args + + +def main() -> int: + # Handle --self-test before full argument parsing so it needs no network/address configuration. + if "--self-test" in sys.argv: + return run_self_test() + args = parse_args() + harness = RtpsHostHarness(args) + harness.run() + return 0 + + +if __name__ == "__main__": + sys.exit(main())