From e5bb33c6daaa75cbd35c53a32ca73df5786f0c07 Mon Sep 17 00:00:00 2001 From: William Emfinger Date: Mon, 15 Jun 2026 14:06:23 -0500 Subject: [PATCH 01/32] feat(rtps, cdr): Add `cdr` and `rtps` components --- .github/workflows/build.yml | 4 + .github/workflows/upload_components.yml | 2 + components/cdr/CMakeLists.txt | 3 + components/cdr/README.md | 34 + components/cdr/example/CMakeLists.txt | 21 + components/cdr/example/README.md | 30 + components/cdr/example/main/CMakeLists.txt | 2 + components/cdr/example/main/cdr_example.cpp | 68 + components/cdr/example/sdkconfig.defaults | 1 + components/cdr/idf_component.yml | 21 + components/cdr/include/cdr.hpp | 518 ++++++ components/cdr/src/cdr.cpp | 1 + components/rtps/CMakeLists.txt | 4 + components/rtps/README.md | 60 + components/rtps/example/CMakeLists.txt | 21 + components/rtps/example/README.md | 61 + components/rtps/example/main/CMakeLists.txt | 3 + .../rtps/example/main/Kconfig.projbuild | 85 + components/rtps/example/main/rtps_example.cpp | 221 +++ components/rtps/example/partitions.csv | 5 + components/rtps/example/sdkconfig.defaults | 13 + components/rtps/idf_component.yml | 26 + components/rtps/include/rtps.hpp | 370 +++++ components/rtps/src/rtps.cpp | 1413 +++++++++++++++++ doc/Doxyfile | 4 + doc/conf_common.py | 6 + doc/en/cdr.rst | 32 + doc/en/cdr_example.md | 2 + doc/en/index.rst | 2 + doc/en/rtps.rst | 259 +++ doc/en/rtps_example.md | 2 + doc/en/rtsp.rst | 109 ++ doc/requirements.txt | 1 + docker_build_docs.sh | 2 + python/README.md | 28 +- python/rtps_host.py | 1022 ++++++++++++ 36 files changed, 4455 insertions(+), 1 deletion(-) create mode 100644 components/cdr/CMakeLists.txt create mode 100644 components/cdr/README.md create mode 100644 components/cdr/example/CMakeLists.txt create mode 100644 components/cdr/example/README.md create mode 100644 components/cdr/example/main/CMakeLists.txt create mode 100644 components/cdr/example/main/cdr_example.cpp create mode 100644 components/cdr/example/sdkconfig.defaults create mode 100644 components/cdr/idf_component.yml create mode 100644 components/cdr/include/cdr.hpp create mode 100644 components/cdr/src/cdr.cpp create mode 100644 components/rtps/CMakeLists.txt create mode 100644 components/rtps/README.md create mode 100644 components/rtps/example/CMakeLists.txt create mode 100644 components/rtps/example/README.md create mode 100644 components/rtps/example/main/CMakeLists.txt create mode 100644 components/rtps/example/main/Kconfig.projbuild create mode 100644 components/rtps/example/main/rtps_example.cpp create mode 100644 components/rtps/example/partitions.csv create mode 100644 components/rtps/example/sdkconfig.defaults create mode 100644 components/rtps/idf_component.yml create mode 100644 components/rtps/include/rtps.hpp create mode 100644 components/rtps/src/rtps.cpp create mode 100644 doc/en/cdr.rst create mode 100644 doc/en/cdr_example.md create mode 100644 doc/en/rtps.rst create mode 100644 doc/en/rtps_example.md create mode 100644 python/rtps_host.py diff --git a/.github/workflows/build.yml b/.github/workflows/build.yml index ed5cdb315..37b030deb 100755 --- a/.github/workflows/build.yml +++ b/.github/workflows/build.yml @@ -53,6 +53,8 @@ jobs: target: esp32s3 - path: 'components/chsc6x/example' target: esp32s3 + - path: 'components/cdr/example' + target: esp32 - path: 'components/cli/example' target: esp32 - path: 'components/cobs/example' @@ -177,6 +179,8 @@ jobs: target: esp32s3 - path: 'components/rmt/example' target: esp32s3 + - path: 'components/rtps/example' + target: esp32 - path: 'components/rtsp/example' target: esp32 - path: 'components/runqueue/example' diff --git a/.github/workflows/upload_components.yml b/.github/workflows/upload_components.yml index 9c62f4b5c..b3946eed4 100755 --- a/.github/workflows/upload_components.yml +++ b/.github/workflows/upload_components.yml @@ -47,6 +47,7 @@ jobs: components/button components/byte90 components/chsc6x + components/cdr components/cli components/cobs components/codec @@ -109,6 +110,7 @@ jobs: components/qwiicnes components/remote_debug components/rmt + components/rtps components/rtsp components/runqueue components/rx8130ce diff --git a/components/cdr/CMakeLists.txt b/components/cdr/CMakeLists.txt new file mode 100644 index 000000000..acbe2799e --- /dev/null +++ b/components/cdr/CMakeLists.txt @@ -0,0 +1,3 @@ +idf_component_register( + INCLUDE_DIRS "include" + SRC_DIRS "src") diff --git a/components/cdr/README.md b/components/cdr/README.md new file mode 100644 index 000000000..3a9e941e9 --- /dev/null +++ b/components/cdr/README.md @@ -0,0 +1,34 @@ +# CDR (Common Data Representation) Component + +[![Badge](https://components.espressif.com/components/espp/cdr/badge.svg)](https://components.espressif.com/components/espp/cdr) + +The `cdr` component provides a small, standalone Common Data Representation +(CDR) reader/writer utility aimed at standards-oriented protocols such as +DDS/RTPS. + +This initial slice is intentionally focused on the most immediately useful +pieces for building interoperable payloads: + +- encapsulation identifiers for `CDR_BE`, `CDR_LE`, `PL_CDR_BE`, and `PL_CDR_LE` +- endian-aware primitive read/write helpers +- CDR alignment and padding handling +- string serialization helpers using the standard CDR length-prefix + null terminator format +- headerless/body helpers for CDR fields embedded inside larger protocol elements +- fixed-array helpers and zero-copy payload/span views +- sequence helpers for homogeneous primitive collections +- standalone usage without depending on RTPS or DDS layers + +Current scope: + +- good fit for building RTPS payloads and parameter lists incrementally +- designed to stay reusable outside DDS/RTPS +- **not** yet a full DDS XTypes / XCDR2 implementation + +## Example + +The [example](./example) demonstrates a small round-trip using: + +- a little-endian CDR encapsulation header +- primitive values +- a CDR string +- a `uint16_t` sequence diff --git a/components/cdr/example/CMakeLists.txt b/components/cdr/example/CMakeLists.txt new file mode 100644 index 000000000..491c40697 --- /dev/null +++ b/components/cdr/example/CMakeLists.txt @@ -0,0 +1,21 @@ +# The following lines of boilerplate have to be in your project's CMakeLists +# in this exact order for cmake to work correctly +cmake_minimum_required(VERSION 3.20) + +set(ENV{IDF_COMPONENT_MANAGER} "0") +include($ENV{IDF_PATH}/tools/cmake/project.cmake) + +set(EXTRA_COMPONENT_DIRS + "../../../components/" +) + +set( + COMPONENTS + "main esptool_py cdr logger" + CACHE STRING + "List of components to include" + ) + +project(cdr_example) + +set(CMAKE_CXX_STANDARD 20) diff --git a/components/cdr/example/README.md b/components/cdr/example/README.md new file mode 100644 index 000000000..85b9fddb0 --- /dev/null +++ b/components/cdr/example/README.md @@ -0,0 +1,30 @@ +# CDR Example + +This example demonstrates a small CDR round-trip using the `cdr` component. + +It exercises: + +- a little-endian CDR encapsulation header +- primitive value serialization +- CDR string serialization +- `uint16_t` sequence serialization +- fixed-array serialization with `write_array` / `read_array` +- headerless/body CDR helpers for embedding fields inside a larger protocol value +- round-trip parsing with `CdrReader` + +## How to use example + +### Build and Flash + +Build the project and flash it to the board, then run monitor tool to view serial output: + +```bash +idf.py -p PORT flash monitor +``` + +Replace `PORT` with the name of the serial port to use. + +## Expected Output + +The example logs the encoded byte count and the decoded values. It finishes by +printing `CDR round-trip succeeded`. diff --git a/components/cdr/example/main/CMakeLists.txt b/components/cdr/example/main/CMakeLists.txt new file mode 100644 index 000000000..a941e22ba --- /dev/null +++ b/components/cdr/example/main/CMakeLists.txt @@ -0,0 +1,2 @@ +idf_component_register(SRC_DIRS "." + INCLUDE_DIRS ".") diff --git a/components/cdr/example/main/cdr_example.cpp b/components/cdr/example/main/cdr_example.cpp new file mode 100644 index 000000000..6bc8662cb --- /dev/null +++ b/components/cdr/example/main/cdr_example.cpp @@ -0,0 +1,68 @@ +#include +#include +#include +#include + +#include "cdr.hpp" +#include "logger.hpp" + +extern "C" void app_main(void) { + espp::Logger logger({.tag = "cdr_example", .level = espp::Logger::Verbosity::INFO}); + + std::array input_magic{'C', 'D', 'R', '!'}; + std::array input_values{10, 20, 30}; + + espp::CdrWriter writer({ + .encapsulation = espp::CdrEncapsulation::CDR_LE, + .include_encapsulation = true, + }); + + // cdr example + writer.write(42); + writer.write(3.25f); + writer.write_string("hello cdr"); + writer.write_sequence(input_values); + + auto payload = writer.take_buffer(); + logger.info("Serialized {} bytes of CDR data", payload.size()); + + auto inline_writer = espp::CdrWriter::make_body_writer(espp::CdrEncapsulation::CDR_LE); + inline_writer.write_array(input_magic); + inline_writer.write_string("embedded field"); + auto inline_payload = + espp::CdrWriter::encapsulate(inline_writer.payload(), espp::CdrEncapsulation::PL_CDR_LE); + + espp::CdrReader reader(payload); + espp::CdrReader inline_reader(inline_payload); + uint32_t decoded_count = 0; + float decoded_scale = 0.0f; + std::string decoded_text; + std::vector decoded_values; + std::array decoded_magic{}; + std::string decoded_inline_text; + + bool ok = reader.read(decoded_count) && reader.read(decoded_scale) && + reader.read_string(decoded_text) && reader.read_sequence(decoded_values); + bool inline_ok = inline_reader.encapsulation() == espp::CdrEncapsulation::PL_CDR_LE && + inline_reader.read_array(decoded_magic) && + inline_reader.read_string(decoded_inline_text); + + if (!ok || !inline_ok) { + logger.error("Failed to decode CDR payload"); + return; + } + + logger.info("Decoded count={}, scale={:.2f}, text='{}', sequence size={}, embedded='{}'", + decoded_count, decoded_scale, decoded_text, decoded_values.size(), + decoded_inline_text); + + if (decoded_count != 42 || decoded_scale != 3.25f || decoded_text != "hello cdr" || + decoded_values.size() != input_values.size() || + !std::equal(decoded_values.begin(), decoded_values.end(), input_values.begin()) || + decoded_magic != input_magic || decoded_inline_text != "embedded field") { + logger.error("CDR round-trip mismatch"); + return; + } + + logger.info("CDR round-trip succeeded"); +} diff --git a/components/cdr/example/sdkconfig.defaults b/components/cdr/example/sdkconfig.defaults new file mode 100644 index 000000000..8a325dde2 --- /dev/null +++ b/components/cdr/example/sdkconfig.defaults @@ -0,0 +1 @@ +# Intentionally minimal; this example only exercises the CDR helpers. diff --git a/components/cdr/idf_component.yml b/components/cdr/idf_component.yml new file mode 100644 index 000000000..92d805db4 --- /dev/null +++ b/components/cdr/idf_component.yml @@ -0,0 +1,21 @@ +## IDF Component Manager Manifest File +license: "MIT" +description: "Common Data Representation (CDR) read/write helpers for ESP-IDF and cross-platform use." +url: "https://github.com/esp-cpp/espp/tree/main/components/cdr" +repository: "git://github.com/esp-cpp/espp.git" +maintainers: + - William Emfinger +documentation: "https://esp-cpp.github.io/espp/cdr.html" +examples: + - path: example +tags: + - cpp + - Component + - CDR + - DDS + - RTPS + - Serialization + - XCDR +dependencies: + idf: + version: '>=5.0' diff --git a/components/cdr/include/cdr.hpp b/components/cdr/include/cdr.hpp new file mode 100644 index 000000000..a3c8a5944 --- /dev/null +++ b/components/cdr/include/cdr.hpp @@ -0,0 +1,518 @@ +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace espp { + +/// Supported CDR encapsulation identifiers. +enum class CdrEncapsulation : uint16_t { + CDR_BE = 0x0000, ///< Big-endian Common Data Representation. + CDR_LE = 0x0001, ///< Little-endian Common Data Representation. + PL_CDR_BE = 0x0002, ///< Big-endian parameter-list CDR. + PL_CDR_LE = 0x0003, ///< Little-endian parameter-list CDR. +}; + +namespace detail { +template inline T swap_endian(T value) { + auto bytes = std::bit_cast>(value); + std::reverse(bytes.begin(), bytes.end()); + return std::bit_cast(bytes); +} + +template inline T convert_endian(T value, bool target_little_endian) { + if constexpr (sizeof(T) == 1) { + return value; + } else { + if ((std::endian::native == std::endian::little) == target_little_endian) { + return value; + } + return swap_endian(value); + } +} + +constexpr size_t cdr_alignment_for_size(size_t size) { + return size >= 8 ? 8 : (size >= 4 ? 4 : (size >= 2 ? 2 : 1)); +} + +template constexpr size_t cdr_alignment() { return cdr_alignment_for_size(sizeof(T)); } +} // namespace detail + +/// Small helper for building CDR/XCDR1-style byte streams. +/// +/// \section cdr_ex1 CDR Example +/// \snippet cdr_example.cpp cdr example +class CdrWriter { +public: + /// @brief Configuration for a CDR writer instance. + struct Config { + CdrEncapsulation encapsulation{ + CdrEncapsulation::CDR_LE}; ///< Encapsulation kind to emit when writing. + bool include_encapsulation{ + true}; ///< If true, prepend the 4-byte encapsulation header to the buffer. + }; + + /// @brief Create a configuration for writing a CDR body without an encapsulation header. + /// @param encapsulation Endianness/encapsulation rules to use for the body payload. + /// @return A configuration with encapsulation emission disabled. + [[nodiscard]] static Config + body_config(CdrEncapsulation encapsulation = CdrEncapsulation::CDR_LE) { + return { + .encapsulation = encapsulation, + .include_encapsulation = false, + }; + } + + /// @brief Create a writer configured for a headerless/body-only CDR payload. + /// @param encapsulation Endianness/encapsulation rules to use for the body payload. + /// @return A ready-to-use writer with no encapsulation header in its output. + [[nodiscard]] static CdrWriter + make_body_writer(CdrEncapsulation encapsulation = CdrEncapsulation::CDR_LE) { + return CdrWriter(body_config(encapsulation)); + } + + /// @brief Wrap an existing payload with a CDR encapsulation header. + /// @param payload Raw bytes to append after the generated encapsulation header. + /// @param encapsulation Encapsulation header to prepend. + /// @return A new byte buffer containing the encapsulation header followed by the payload. + [[nodiscard]] static std::vector + encapsulate(std::span payload, + CdrEncapsulation encapsulation = CdrEncapsulation::CDR_LE) { + CdrWriter writer({ + .encapsulation = encapsulation, + .include_encapsulation = true, + }); + writer.write_bytes(payload); + return writer.take_buffer(); + } + + /// @brief Construct a writer using the default little-endian encapsulated configuration. + CdrWriter() { reset(); } + + /// @brief Construct a writer using an explicit configuration. + /// @param config Writer configuration controlling encapsulation and endianness behavior. + explicit CdrWriter(const Config &config) + : config_(config) { + reset(); + } + + /// @brief Clear the current buffer and reinitialize the encapsulation header if configured. + void reset() { + data_.clear(); + if (config_.include_encapsulation) { + auto value = static_cast(config_.encapsulation); + data_.push_back(static_cast((value >> 8) & 0xff)); + data_.push_back(static_cast(value & 0xff)); + data_.push_back(0); + data_.push_back(0); + } + } + + /// @brief Get the configured encapsulation kind for this writer. + /// @return The encapsulation kind associated with this writer. + [[nodiscard]] CdrEncapsulation encapsulation() const { return config_.encapsulation; } + + /// @brief Determine whether values are encoded in little-endian order. + /// @return True for little-endian encapsulations, false for big-endian ones. + [[nodiscard]] bool uses_little_endian() const { + return config_.encapsulation == CdrEncapsulation::CDR_LE || + config_.encapsulation == CdrEncapsulation::PL_CDR_LE; + } + + /// @brief Get the total number of bytes currently written. + /// @return The size of the backing byte buffer, including any encapsulation header. + [[nodiscard]] size_t size() const { return data_.size(); } + + /// @brief Access the full serialized buffer built so far. + /// @return A const reference to the complete buffer, including any encapsulation header. + [[nodiscard]] const std::vector &buffer() const { return data_; } + + /// @brief Access only the payload portion of the buffer. + /// @return A view over the serialized bytes after any encapsulation header. + [[nodiscard]] std::span payload() const { + auto bytes = std::span{data_.data(), data_.size()}; + return bytes.subspan(std::min(payload_offset(), bytes.size())); + } + + /// @brief Move the complete serialized buffer out of the writer. + /// @return The current buffer contents, including any encapsulation header. + [[nodiscard]] std::vector take_buffer() { return std::move(data_); } + + /// @brief Pad the buffer with zeros until it satisfies the requested alignment. + /// @param alignment Required alignment in bytes. Values less than or equal to 1 are ignored. + void align(size_t alignment) { + if (alignment <= 1) { + return; + } + while (data_.size() % alignment != 0) { + data_.push_back(0); + } + } + + /// @brief Append a primitive scalar using CDR alignment and endianness rules. + /// @tparam T Primitive integral or floating-point type to encode. + /// @param value Value to append to the serialized buffer. + /// @return True after the value has been encoded and appended. + template + requires(std::is_integral_v || std::is_floating_point_v) bool write(T value) { + align(detail::cdr_alignment()); + auto encoded = detail::convert_endian(value, uses_little_endian()); + auto bytes = std::bit_cast>(encoded); + auto *raw = reinterpret_cast(bytes.data()); + data_.insert(data_.end(), raw, raw + bytes.size()); + return true; + } + + /// @brief Append a boolean value using the standard CDR 1-byte representation. + /// @param value Boolean value to encode. + /// @return True after the value has been appended. + bool write_bool(bool value) { return write(value ? 1 : 0); } + + /// @brief Append a CDR string. + /// @param text UTF-8 text to encode. A terminating null byte is written automatically. + /// @return True after the string length, contents, terminator, and alignment padding are written. + bool write_string(std::string_view text) { + align(4); + write(static_cast(text.size() + 1)); + data_.insert(data_.end(), text.begin(), text.end()); + data_.push_back(0); + align(4); + return true; + } + + /// @brief Append raw bytes with optional alignment. + /// @param bytes Bytes to copy into the serialized buffer. + /// @param alignment Alignment in bytes to satisfy before appending the data. + /// @return True after the bytes have been appended. + bool write_bytes(std::span bytes, size_t alignment = 1) { + align(alignment); + data_.insert(data_.end(), bytes.begin(), bytes.end()); + return true; + } + + /// @brief Append a fixed-size array of primitive values. + /// @tparam T Primitive integral or floating-point element type. + /// @tparam N Number of elements in the array. + /// @param values Array to encode element-by-element. + /// @return True after all elements have been encoded. + template + requires(std::is_integral_v || + std::is_floating_point_v) bool write_array(const std::array &values) { + for (const auto &value : values) { + if (!write(value)) { + return false; + } + } + return true; + } + + /// @brief Append a variable-length CDR sequence of primitive values. + /// @tparam T Primitive integral or floating-point element type. + /// @param values Sequence elements to encode. + /// @return True after the sequence length and all elements have been encoded. + template + requires(std::is_integral_v || + std::is_floating_point_v) bool write_sequence(std::span values) { + align(4); + write(static_cast(values.size())); + for (const auto &value : values) { + write(value); + } + return true; + } + +private: + [[nodiscard]] size_t payload_offset() const { return config_.include_encapsulation ? 4 : 0; } + + Config config_; + std::vector data_{}; +}; + +/// Small helper for parsing CDR/XCDR1-style byte streams. +class CdrReader { +public: + /// @brief Configuration for a CDR reader instance. + struct Config { + bool expect_encapsulation{ + true}; ///< If true, consume and validate a 4-byte encapsulation header on reset. + CdrEncapsulation default_encapsulation{ + CdrEncapsulation::CDR_LE}; ///< Encapsulation to assume when no header is expected. + }; + + /// @brief Create a configuration for reading a headerless/body-only CDR payload. + /// @param encapsulation Encapsulation/endian rules to assume for the payload body. + /// @return A configuration with encapsulation consumption disabled. + [[nodiscard]] static Config + body_config(CdrEncapsulation encapsulation = CdrEncapsulation::CDR_LE) { + return { + .expect_encapsulation = false, + .default_encapsulation = encapsulation, + }; + } + + /// @brief Create a reader for a headerless/body-only CDR payload. + /// @param data Serialized payload bytes to read. + /// @param encapsulation Encapsulation/endian rules to assume for the payload body. + /// @return A reader initialized for body-only parsing. + [[nodiscard]] static CdrReader + make_body_reader(std::span data, + CdrEncapsulation encapsulation = CdrEncapsulation::CDR_LE) { + return CdrReader(data, body_config(encapsulation)); + } + + /// @brief Construct a reader that expects a standard encapsulated CDR payload. + /// @param data Serialized bytes to parse. + explicit CdrReader(std::span data) { reset(data); } + + /// @brief Construct a reader with an explicit configuration. + /// @param data Serialized bytes to parse. + /// @param config Reader configuration controlling encapsulation handling. + CdrReader(std::span data, const Config &config) + : config_(config) { + reset(data); + } + + /// @brief Reset the reader to the beginning of a new serialized buffer. + /// @param data Serialized bytes to parse. + void reset(std::span data) { + data_ = data; + offset_ = 0; + valid_ = true; + if (config_.expect_encapsulation) { + if (data_.size() < 4) { + valid_ = false; + return; + } + uint16_t raw = static_cast(data_[0] << 8) | static_cast(data_[1]); + switch (raw) { + case static_cast(CdrEncapsulation::CDR_BE): + case static_cast(CdrEncapsulation::CDR_LE): + case static_cast(CdrEncapsulation::PL_CDR_BE): + case static_cast(CdrEncapsulation::PL_CDR_LE): + encapsulation_ = static_cast(raw); + break; + default: + valid_ = false; + return; + } + offset_ = 4; + } else { + encapsulation_ = config_.default_encapsulation; + } + } + + /// @brief Check whether the reader is still in a valid state. + /// @return True if parsing can continue, false if a prior operation failed. + [[nodiscard]] bool valid() const { return valid_; } + + /// @brief Get the active encapsulation for the current buffer. + /// @return The parsed or assumed encapsulation value. + [[nodiscard]] CdrEncapsulation encapsulation() const { return encapsulation_; } + + /// @brief Determine whether values are decoded as little-endian. + /// @return True for little-endian encapsulations, false for big-endian ones. + [[nodiscard]] bool uses_little_endian() const { + return encapsulation_ == CdrEncapsulation::CDR_LE || + encapsulation_ == CdrEncapsulation::PL_CDR_LE; + } + + /// @brief Access only the payload bytes after any encapsulation header. + /// @return A view over the unread buffer excluding the encapsulation header. + [[nodiscard]] std::span payload() const { + return data_.subspan(std::min(payload_offset(), data_.size())); + } + + /// @brief Get the number of unread bytes remaining. + /// @return Remaining unread byte count, or 0 if the offset is beyond the buffer. + [[nodiscard]] size_t remaining() const { + return offset_ <= data_.size() ? data_.size() - offset_ : 0; + } + + /// @brief Access a view of the unread bytes without copying. + /// @return A span over the unread tail of the buffer, or an empty span if the reader is invalid. + [[nodiscard]] std::span remaining_view() const { + if (!valid_) { + return {}; + } + return data_.subspan(std::min(offset_, data_.size())); + } + + /// @brief Advance the read cursor by a fixed number of bytes. + /// @param length Number of bytes to skip. + /// @return True if the bytes were skipped, false if the reader became invalid. + bool skip(size_t length) { + if (!valid_ || remaining() < length) { + valid_ = false; + return false; + } + offset_ += length; + return true; + } + + /// @brief Advance the read cursor to satisfy an alignment requirement. + /// @param alignment Required alignment in bytes. Values less than or equal to 1 are ignored. + /// @return True if the padding bytes were skipped successfully, false if the reader became + /// invalid. + bool align(size_t alignment) { + if (alignment <= 1) { + return true; + } + size_t padding = (alignment - (offset_ % alignment)) % alignment; + return skip(padding); + } + + /// @brief Read a primitive scalar using CDR alignment and endianness rules. + /// @tparam T Primitive integral or floating-point type to decode. + /// @param value Output variable that receives the decoded value on success. + /// @return True if a complete value was decoded, false otherwise. + template + requires(std::is_integral_v || std::is_floating_point_v) bool read(T &value) { + if (!align(detail::cdr_alignment()) || remaining() < sizeof(T)) { + valid_ = false; + return false; + } + std::array bytes{}; + std::memcpy(bytes.data(), data_.data() + offset_, sizeof(T)); + offset_ += sizeof(T); + auto encoded = std::bit_cast(bytes); + value = detail::convert_endian(encoded, uses_little_endian()); + return true; + } + + /// @brief Read a boolean value encoded with the CDR 1-byte representation. + /// @param value Output variable that receives the decoded boolean on success. + /// @return True if the boolean was read successfully, false otherwise. + bool read_bool(bool &value) { + uint8_t raw = 0; + if (!read(raw)) { + return false; + } + value = raw != 0; + return true; + } + + /// @brief Read a CDR string. + /// @param text Output string receiving the decoded text without the trailing null terminator. + /// @return True if the string was decoded successfully, false otherwise. + bool read_string(std::string &text) { + if (!align(4)) { + return false; + } + uint32_t length = 0; + if (!read(length) || length == 0 || remaining() < length) { + valid_ = false; + return false; + } + auto span = data_.subspan(offset_, length); + offset_ += length; + if (span.back() == 0) { + span = span.first(span.size() - 1); + } + text.assign(reinterpret_cast(span.data()), span.size()); + return align(4); + } + + /// @brief Read a fixed number of bytes as a zero-copy span. + /// @param length Number of bytes to expose. + /// @param alignment Alignment in bytes to satisfy before reading. + /// @return A span over the requested bytes, or an empty span if the read failed. + std::span read_span(size_t length, size_t alignment = 1) { + if (!align(alignment) || remaining() < length) { + valid_ = false; + return {}; + } + auto span = data_.subspan(offset_, length); + offset_ += length; + return span; + } + + /// @brief Read bytes into a caller-provided mutable span. + /// @param bytes Destination span that receives the copied bytes. + /// @param alignment Alignment in bytes to satisfy before reading. + /// @return True if all requested bytes were read successfully, false otherwise. + bool read_bytes(std::span bytes, size_t alignment = 1) { + auto span = read_span(bytes.size(), alignment); + if (span.size() != bytes.size()) { + return false; + } + std::memcpy(bytes.data(), span.data(), bytes.size()); + return true; + } + + /// @brief Read bytes into a vector. + /// @param bytes Output vector replaced with the decoded bytes on success. + /// @param length Number of bytes to read. + /// @param alignment Alignment in bytes to satisfy before reading. + /// @return True if the requested bytes were read successfully, false otherwise. + bool read_bytes(std::vector &bytes, size_t length, size_t alignment = 1) { + auto span = read_span(length, alignment); + if (span.size() != length) { + return false; + } + bytes.assign(span.begin(), span.end()); + return true; + } + + /// @brief Read a fixed-size array of primitive values. + /// @tparam T Primitive integral or floating-point element type. + /// @tparam N Number of elements in the array. + /// @param values Output array receiving the decoded elements. + /// @return True if all array elements were read successfully, false otherwise. + template + requires(std::is_integral_v || + std::is_floating_point_v) bool read_array(std::array &values) { + for (auto &value : values) { + if (!read(value)) { + return false; + } + } + return true; + } + + /// @brief Read a variable-length CDR sequence of primitive values. + /// @tparam T Primitive integral or floating-point element type. + /// @param values Output vector replaced with the decoded sequence elements on success. + /// @return True if the sequence length and all elements were decoded successfully, false + /// otherwise. + template + requires(std::is_integral_v || + std::is_floating_point_v) bool read_sequence(std::vector &values) { + if (!align(4)) { + return false; + } + uint32_t length = 0; + if (!read(length)) { + return false; + } + values.clear(); + values.reserve(length); + for (uint32_t i = 0; i < length; i++) { + T value{}; + if (!read(value)) { + return false; + } + values.push_back(value); + } + return true; + } + +private: + [[nodiscard]] size_t payload_offset() const { return config_.expect_encapsulation ? 4 : 0; } + + Config config_{}; + std::span data_{}; + size_t offset_{0}; + bool valid_{false}; + CdrEncapsulation encapsulation_{CdrEncapsulation::CDR_LE}; +}; + +} // namespace espp diff --git a/components/cdr/src/cdr.cpp b/components/cdr/src/cdr.cpp new file mode 100644 index 000000000..584ad00cc --- /dev/null +++ b/components/cdr/src/cdr.cpp @@ -0,0 +1 @@ +#include "cdr.hpp" diff --git a/components/rtps/CMakeLists.txt b/components/rtps/CMakeLists.txt new file mode 100644 index 000000000..1dc01861d --- /dev/null +++ b/components/rtps/CMakeLists.txt @@ -0,0 +1,4 @@ +idf_component_register( + INCLUDE_DIRS "include" + SRC_DIRS "src" + REQUIRES base_component cdr task socket) diff --git a/components/rtps/README.md b/components/rtps/README.md new file mode 100644 index 000000000..05ec19090 --- /dev/null +++ b/components/rtps/README.md @@ -0,0 +1,60 @@ +# RTPS Component + +[![Badge](https://components.espressif.com/components/espp/rtps/badge.svg)](https://components.espressif.com/components/espp/rtps) + +The `RtpsParticipant` component is the beginning of a cross-platform RTPS +(Real-Time Publish-Subscribe) implementation built on top of the ESPP `socket` +component. + +This component now includes the first real RTPS discovery slice: + +- RTPS header and DATA submessage framing helpers +- standard RTPS UDPv4 port calculations +- GUID, entity ID, locator, and sequence number utility types +- SPDP participant announcements using PL_CDR parameter lists +- SEDP publication and subscription announcements for local endpoints +- parsing and tracking of discovered remote participants, writers, and readers +- integration with the shared `cdr` component for CDR/PL_CDR payload handling + +The long-term goal for this component is DDS/RTPS interoperability with ROS 2 +nodes, including best-effort and reliable user-data flows. Discovery is now +standards-shaped, but the reliable RTPS state machines (`HEARTBEAT`, +`ACKNACK`, resend windows) and ROS 2 endpoint/user-data interoperability are +still incomplete. + +## Expected Compatibility + +The table below is intentionally conservative: **expected** means "this is the +intended scope based on the current wire format and code", not "fully verified +against every stack". + +| Peer implementation | Expected compatibility | Notes | +| --- | --- | --- | +| ESPP `rtps` component / `python/rtps_host.py` | **Yes** for current scaffold | Intended smoke-test path for SPDP, SEDP, and the temporary `UInt32` `ESPPDATA` user-data payload. | +| Generic DDSI-RTPS 2.3 implementations | **Partial** | SPDP and SEDP messages are standards-shaped, but only the discovery slice is implemented today. | +| ROS 2 nodes backed by Fast DDS | **Partial / discovery-targeted** | The current discovery messages include ROS 2-relevant participant user data such as `enclave=...;`, but standards-based ROS 2 topic data exchange is not finished yet. | +| ROS 2 nodes backed by Cyclone DDS or other DDS vendors | **Partial / unverified** | Expected to be limited to the minimal discovery subset if the peer accepts the currently emitted parameter set; not validated yet. | +| Reliable DDS/RTPS endpoints | **No** | `HEARTBEAT`, `ACKNACK`, retransmission windows, and other reliable state-machine pieces are not implemented. | + +## Feature Status + +| Feature | Status | Notes | +| --- | --- | --- | +| RTPS header / DATA submessage serialize + parse | **Implemented** | Core message framing is present. | +| Standard UDPv4 RTPS port mapping | **Implemented** | Uses the DDSI-RTPS well-known port formula. | +| SPDP participant announce send/receive | **Implemented** | Multicast announce plus participant cache updates. | +| SEDP publication / subscription announce send/receive | **Implemented** | Local endpoints are announced and remote endpoints are cached. | +| Participant / endpoint discovery callbacks | **Implemented** | Exposed through `on_participant_discovered` and `on_endpoint_discovered`. | +| Temporary `UInt32` user-data path | **Implemented** | Uses the current ESPP-specific `ESPPDATA` payload, not a standards-based DDS sample representation. | +| QoS fields emitted in discovery | **Partial** | Reliability, durability, liveliness, and history parameters are advertised in SEDP. | +| QoS matching / policy enforcement | **Not implemented** | Remote QoS is parsed, but full writer/reader matching logic is still missing. | +| Standards-based DDS user-data serialization | **Not implemented** | The current data path is a temporary ESPP scaffold for `std_msgs/msg/UInt32`. | +| Inline QoS handling | **Not implemented** | Discovery and user-data handling assume no inline QoS. | +| Reliable RTPS (`HEARTBEAT`, `ACKNACK`, resend`) | **Not implemented** | Reliable delivery is not interoperable yet. | +| Full ROS 2 topic interoperability | **Not implemented** | Discovery is the current milestone; ROS 2-compatible data writers/readers are still pending. | + +## Example + +The [example](./example) exercises the protocol helpers, computes the standard +RTPS ports, builds/parses SPDP and SEDP messages, and demonstrates the +participant API without requiring a second device. diff --git a/components/rtps/example/CMakeLists.txt b/components/rtps/example/CMakeLists.txt new file mode 100644 index 000000000..e64b1c087 --- /dev/null +++ b/components/rtps/example/CMakeLists.txt @@ -0,0 +1,21 @@ +# The following lines of boilerplate have to be in your project's CMakeLists +# in this exact order for cmake to work correctly +cmake_minimum_required(VERSION 3.20) + +set(ENV{IDF_COMPONENT_MANAGER} "0") +include($ENV{IDF_PATH}/tools/cmake/project.cmake) + +set(EXTRA_COMPONENT_DIRS + "../../../components/" +) + +set( + COMPONENTS + "main esptool_py logger rtps wifi" + CACHE STRING + "List of components to include" + ) + +project(rtps_example) + +set(CMAKE_CXX_STANDARD 20) diff --git a/components/rtps/example/README.md b/components/rtps/example/README.md new file mode 100644 index 000000000..11f5259ee --- /dev/null +++ b/components/rtps/example/README.md @@ -0,0 +1,61 @@ +# RTPS Example + +This example now acts as a two-node RTPS smoke test for ESP targets on the same +Wi-Fi network. + +It demonstrates: + +* Wi-Fi STA setup for host-network RTPS traffic +* standard RTPS UDP port calculation for each participant +* SPDP participant discovery between two boards +* SEDP endpoint discovery for request/response topics +* CDR little-endian serialization for `std_msgs/msg/UInt32`-style payloads +* best-effort inter-node request/response sample exchange + +The component's long-term goal is ROS 2 interoperability over DDS/RTPS. This +example focuses on proving cross-board discovery and user-data delivery using +the current scaffold. + +## How to use example + +### Configure two boards + +Build one board as the **initiator** and the other as the **responder**. + +For both boards: + +1. Set the same `RTPS domain ID`, `Topic prefix`, `WiFi SSID`, and `WiFi password`. +2. Give each board a unique `RTPS participant ID`. +3. Optionally set distinct `Participant node name` values to make discovery logs easier to read. + +For one board only: + +1. Select `RTPS Example Configuration -> Example Role -> Initiator` + +For the other board: + +1. Select `RTPS Example Configuration -> Example Role -> Responder` + +### Build and Flash + +Build the project and flash it to the board, then run monitor tool to view +serial output: + +```sh +idf.py -p PORT flash monitor +``` + +(Replace PORT with the name of the serial port to use.) + +## Example Output + +The initiator waits until it discovers the responder's endpoints, then publishes +incrementing values on `/request`. The responder logs each +received request and echoes the same value back on `/response`. + +Expected signs of success: + +* both boards report Wi-Fi connection and their local IP address +* both boards log RTPS participant and endpoint discovery +* the initiator logs `Published request N` followed by `Received response N` +* the responder logs `Received request N, sending response` diff --git a/components/rtps/example/main/CMakeLists.txt b/components/rtps/example/main/CMakeLists.txt new file mode 100644 index 000000000..e398cc2f0 --- /dev/null +++ b/components/rtps/example/main/CMakeLists.txt @@ -0,0 +1,3 @@ +idf_component_register(SRC_DIRS "." + INCLUDE_DIRS "." + REQUIRES logger rtps wifi) diff --git a/components/rtps/example/main/Kconfig.projbuild b/components/rtps/example/main/Kconfig.projbuild new file mode 100644 index 000000000..ff2656842 --- /dev/null +++ b/components/rtps/example/main/Kconfig.projbuild @@ -0,0 +1,85 @@ +menu "RTPS Example Configuration" + + choice RTPS_EXAMPLE_ROLE + prompt "Example Role" + default RTPS_EXAMPLE_ROLE_INITIATOR + help + Build one board as the initiator and a second board as the responder + to verify RTPS discovery and end-to-end UInt32 sample exchange. + + config RTPS_EXAMPLE_ROLE_INITIATOR + bool "Initiator" + help + Publishes incrementing request samples after it discovers a + responder, then logs the matching responses. + + config RTPS_EXAMPLE_ROLE_RESPONDER + bool "Responder" + help + Waits for request samples and echoes the same value back on the + response topic. + + endchoice + + config RTPS_EXAMPLE_NODE_NAME + string "Participant node name" + default "espp_rtps_node" + help + Logical RTPS participant name announced during discovery. + + config RTPS_EXAMPLE_DOMAIN_ID + int "RTPS domain ID" + range 0 232 + default 0 + help + Both boards must use the same domain ID to discover each other. + + config RTPS_EXAMPLE_PARTICIPANT_ID + int "RTPS participant ID" + range 0 119 + default 1 + help + Each board should use a unique participant ID within the same domain. + + config RTPS_EXAMPLE_TOPIC_PREFIX + string "Topic prefix" + default "espp/rtps_example" + help + Prefix used to derive the request and response topics. + + config RTPS_EXAMPLE_ANNOUNCE_PERIOD_MS + int "Discovery announce period (ms)" + range 200 10000 + default 1500 + help + Period between periodic SPDP/SEDP discovery announcements. + + config RTPS_EXAMPLE_PUBLISH_PERIOD_MS + int "Initiator publish period (ms)" + range 250 60000 + default 2000 + depends on RTPS_EXAMPLE_ROLE_INITIATOR + help + Period between request messages sent by the initiator after a + responder has been discovered. + + config ESP_WIFI_SSID + string "WiFi SSID" + default "" + help + SSID (network name) for the example to connect to. + + config ESP_WIFI_PASSWORD + string "WiFi Password" + default "" + help + WiFi password (WPA or WPA2) for the example to use. + + config ESP_MAXIMUM_RETRY + int "Maximum retry" + default 5 + help + Set the maximum retry count to avoid reconnecting forever when the + network is unavailable. + +endmenu diff --git a/components/rtps/example/main/rtps_example.cpp b/components/rtps/example/main/rtps_example.cpp new file mode 100644 index 000000000..2abce874f --- /dev/null +++ b/components/rtps/example/main/rtps_example.cpp @@ -0,0 +1,221 @@ +#include +#include +#include +#include + +#include "logger.hpp" +#include "rtps.hpp" +#include "wifi_sta.hpp" + +using namespace std::chrono_literals; + +namespace { +constexpr std::string_view kTypeName = "std_msgs/msg/UInt32"; + +bool run_local_protocol_checks(espp::Logger &logger, const espp::RtpsParticipant &participant) { + auto announce_message = participant.build_announce_message(); + auto parsed_message = espp::RtpsParticipant::Message::parse(announce_message); + if (!parsed_message) { + logger.error("Failed to parse locally built announce message"); + return false; + } + logger.info("Built and parsed SPDP announce message with {} submessage(s)", + parsed_message->submessages.size()); + + if (!participant.writers().empty()) { + auto sedp_publication_message = + participant.build_sedp_publication_message(participant.writers().front()); + auto parsed_publication_message = + espp::RtpsParticipant::Message::parse(sedp_publication_message); + if (!parsed_publication_message) { + logger.error("Failed to parse locally built SEDP publication message"); + return false; + } + logger.info("Built and parsed SEDP publication message with {} submessage(s)", + parsed_publication_message->submessages.size()); + } + + if (!participant.readers().empty()) { + auto sedp_subscription_message = + participant.build_sedp_subscription_message(participant.readers().front()); + auto parsed_subscription_message = + espp::RtpsParticipant::Message::parse(sedp_subscription_message); + if (!parsed_subscription_message) { + logger.error("Failed to parse locally built SEDP subscription message"); + return false; + } + logger.info("Built and parsed SEDP subscription message with {} submessage(s)", + parsed_subscription_message->submessages.size()); + } + + auto uint32_payload = espp::RtpsParticipant::serialize_uint32_cdr(42); + auto maybe_value = espp::RtpsParticipant::deserialize_uint32_cdr(uint32_payload); + if (!maybe_value || *maybe_value != 42) { + logger.error("UInt32 CDR round trip failed"); + return false; + } + logger.info("UInt32 CDR round trip succeeded with value {}", *maybe_value); + return true; +} + +bool has_endpoint(std::span endpoints, + std::string_view topic_name, bool is_reader) { + return std::any_of(endpoints.begin(), endpoints.end(), + [topic_name, is_reader](const auto &endpoint) { + return endpoint.topic_name == topic_name && endpoint.is_reader == is_reader; + }); +} +} // namespace + +extern "C" void app_main(void) { + espp::Logger logger({.tag = "rtps_example", .level = espp::Logger::Verbosity::INFO}); + + std::string ip_address; + espp::WifiSta wifi_sta({.ssid = CONFIG_ESP_WIFI_SSID, + .password = CONFIG_ESP_WIFI_PASSWORD, + .num_connect_retries = CONFIG_ESP_MAXIMUM_RETRY, + .on_connected = nullptr, + .on_disconnected = nullptr, + .on_got_ip = [&ip_address](ip_event_got_ip_t *eventdata) { + ip_address = fmt::format("{}.{}.{}.{}", IP2STR(&eventdata->ip_info.ip)); + fmt::print("got IP: {}\n", ip_address); + }}); + + logger.info("Waiting for WiFi connection..."); + while (!wifi_sta.is_connected()) { + std::this_thread::sleep_for(100ms); + } + logger.info("WiFi connected, local IP {}", ip_address); + + const std::string node_name = CONFIG_RTPS_EXAMPLE_NODE_NAME; + const std::string topic_prefix = CONFIG_RTPS_EXAMPLE_TOPIC_PREFIX; + const std::string request_topic = topic_prefix + "/request"; + const std::string response_topic = topic_prefix + "/response"; + + std::atomic request_count{0}; + std::atomic response_count{0}; + std::atomic next_request_value{1}; + std::atomic last_sent_request{0}; + espp::RtpsParticipant *participant_ptr = nullptr; + + espp::RtpsParticipant participant({ + .node_name = node_name, + .domain_id = CONFIG_RTPS_EXAMPLE_DOMAIN_ID, + .participant_id = CONFIG_RTPS_EXAMPLE_PARTICIPANT_ID, + .advertised_address = ip_address, + .announce_period = std::chrono::milliseconds(CONFIG_RTPS_EXAMPLE_ANNOUNCE_PERIOD_MS), + .on_participant_discovered = + [&logger](const auto &proxy) { + logger.info("Discovered participant '{}' at {} (meta {}, user {})", + proxy.name.empty() ? proxy.guid_prefix.to_string() : proxy.name, + proxy.address, proxy.ports.metatraffic_unicast, proxy.ports.user_unicast); + }, + .on_endpoint_discovered = + [&logger](const auto &endpoint) { + logger.info("Discovered remote {} '{}' [{}]", endpoint.is_reader ? "reader" : "writer", + endpoint.topic_name, endpoint.type_name); + }, + .log_level = espp::Logger::Verbosity::INFO, + }); + participant_ptr = &participant; + +#if CONFIG_RTPS_EXAMPLE_ROLE_INITIATOR + participant.add_writer({ + .topic_name = request_topic, + .type_name = std::string(kTypeName), + .reliability = espp::RtpsParticipant::ReliabilityKind::BEST_EFFORT, + .entity_index = 0, + }); + participant.add_reader({ + .topic_name = response_topic, + .type_name = std::string(kTypeName), + .reliability = espp::RtpsParticipant::ReliabilityKind::BEST_EFFORT, + .entity_index = 0, + .on_uint32_sample = + [&logger, &response_count, &last_sent_request](uint32_t value) { + response_count++; + logger.info("Received response {} (expected {})", value, last_sent_request.load()); + }, + }); +#else + participant.add_writer({ + .topic_name = response_topic, + .type_name = std::string(kTypeName), + .reliability = espp::RtpsParticipant::ReliabilityKind::BEST_EFFORT, + .entity_index = 0, + }); + participant.add_reader({ + .topic_name = request_topic, + .type_name = std::string(kTypeName), + .reliability = espp::RtpsParticipant::ReliabilityKind::BEST_EFFORT, + .entity_index = 0, + .on_uint32_sample = + [&logger, &request_count, &response_topic, &participant_ptr](uint32_t value) { + request_count++; + logger.info("Received request {}, sending response", value); + if (!participant_ptr->publish_uint32(response_topic, value)) { + logger.warn("Failed to publish response {}", value); + } + }, + }); +#endif + + auto ports = participant.ports(); + logger.info("Role: {}", +#if CONFIG_RTPS_EXAMPLE_ROLE_INITIATOR + "initiator" +#else + "responder" +#endif + ); + logger.info("Participant name: {}", node_name); + logger.info("Participant GUID: {}", participant.participant_guid().to_string()); + logger.info("Domain ID: {}, Participant ID: {}", CONFIG_RTPS_EXAMPLE_DOMAIN_ID, + CONFIG_RTPS_EXAMPLE_PARTICIPANT_ID); + logger.info("Topic prefix: {}", topic_prefix); + logger.info("Request topic: {}, Response topic: {}", request_topic, response_topic); + logger.info("Ports: meta mc={}, meta uc={}, user mc={}, user uc={}", ports.metatraffic_multicast, + ports.metatraffic_unicast, ports.user_multicast, ports.user_unicast); + + if (!run_local_protocol_checks(logger, participant)) { + return; + } + + if (!participant.start()) { + logger.error("Failed to start RTPS participant"); + return; + } + +#if CONFIG_RTPS_EXAMPLE_ROLE_INITIATOR + logger.info("Initiator is waiting for a responder on the same domain/topic prefix..."); + while (true) { + auto remote_readers = participant.discovered_readers(); + auto remote_writers = participant.discovered_writers(); + bool request_reader_ready = has_endpoint(remote_readers, request_topic, true); + bool response_writer_ready = has_endpoint(remote_writers, response_topic, false); + if (!request_reader_ready || !response_writer_ready) { + logger.info("Waiting for responder endpoints (request_reader={}, response_writer={})", + request_reader_ready, response_writer_ready); + std::this_thread::sleep_for(2s); + continue; + } + + auto value = next_request_value.fetch_add(1); + last_sent_request = value; + if (participant.publish_uint32(request_topic, value)) { + logger.info("Published request {} on '{}'", value, request_topic); + } else { + logger.warn("Failed to publish request {}", value); + } + std::this_thread::sleep_for(std::chrono::milliseconds(CONFIG_RTPS_EXAMPLE_PUBLISH_PERIOD_MS)); + } +#else + logger.info("Responder is ready and will echo '{}' samples back on '{}'", request_topic, + response_topic); + while (true) { + std::this_thread::sleep_for(5s); + logger.info("Responder status: discovered participants={}, requests handled={}", + participant.discovered_participants().size(), request_count.load()); + } +#endif +} diff --git a/components/rtps/example/partitions.csv b/components/rtps/example/partitions.csv new file mode 100644 index 000000000..c4217ab9e --- /dev/null +++ b/components/rtps/example/partitions.csv @@ -0,0 +1,5 @@ +# ESP-IDF Partition Table +# Name, Type, SubType, Offset, Size, Flags +nvs, data, nvs, 0x9000, 0x6000, +phy_init, data, phy, 0xf000, 0x1000, +factory, app, factory, 0x10000, 1500K, diff --git a/components/rtps/example/sdkconfig.defaults b/components/rtps/example/sdkconfig.defaults new file mode 100644 index 000000000..e823df850 --- /dev/null +++ b/components/rtps/example/sdkconfig.defaults @@ -0,0 +1,13 @@ +# Common ESP-related +# +CONFIG_ESP_SYSTEM_EVENT_TASK_STACK_SIZE=4096 +CONFIG_ESP_MAIN_TASK_STACK_SIZE=8192 + +# +# Partition Table +# +CONFIG_PARTITION_TABLE_CUSTOM=y +CONFIG_PARTITION_TABLE_CUSTOM_FILENAME="partitions.csv" +CONFIG_PARTITION_TABLE_FILENAME="partitions.csv" +CONFIG_PARTITION_TABLE_OFFSET=0x8000 +CONFIG_PARTITION_TABLE_MD5=y diff --git a/components/rtps/idf_component.yml b/components/rtps/idf_component.yml new file mode 100644 index 000000000..094c82efa --- /dev/null +++ b/components/rtps/idf_component.yml @@ -0,0 +1,26 @@ +## IDF Component Manager Manifest File +license: "MIT" +description: "Cross-platform RTPS protocol foundation component for ESP-IDF" +url: "https://github.com/esp-cpp/espp/tree/main/components/rtps" +repository: "git://github.com/esp-cpp/espp.git" +maintainers: + - William Emfinger +documentation: "https://esp-cpp.github.io/espp/rtps.html" +examples: + - path: example +tags: + - cpp + - Component + - RTPS + - DDS + - ROS2 + - UDP + - Discovery + - PubSub +dependencies: + idf: + version: '>=5.0' + espp/base_component: '>=1.0' + espp/cdr: '>=1.0' + espp/socket: '>=1.0' + espp/task: '>=1.0' diff --git a/components/rtps/include/rtps.hpp b/components/rtps/include/rtps.hpp new file mode 100644 index 000000000..6e8a308da --- /dev/null +++ b/components/rtps/include/rtps.hpp @@ -0,0 +1,370 @@ +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "base_component.hpp" +#include "task.hpp" +#include "udp_socket.hpp" + +namespace espp { +/// Cross-platform RTPS protocol foundation built on top of the socket component. +class RtpsParticipant : public BaseComponent { +public: + /// @brief Delivery semantics advertised for a writer or reader endpoint. + enum class ReliabilityKind : uint8_t { + BEST_EFFORT = 0, ///< Best-effort delivery semantics. + RELIABLE = 1, ///< Reliable delivery semantics. + }; + + /// @brief RTPS protocol version carried in the RTPS message header. + struct ProtocolVersion { + uint8_t major{2}; ///< Major RTPS version number. + uint8_t minor{3}; ///< Minor RTPS version number. + }; + + /// @brief RTPS vendor identifier carried in the RTPS message header. + struct VendorId { + std::array value{0xca, 0xfe}; ///< Two-byte vendor identifier. + }; + + /// @brief 12-byte prefix that identifies an RTPS participant. + struct GuidPrefix { + std::array value{}; ///< Raw 12-byte GUID prefix value. + + /// @brief Compare two GUID prefixes for equality. + /// @param other Prefix to compare against. + /// @return True if both prefixes contain identical bytes. + bool operator==(const GuidPrefix &other) const = default; + + /// @brief Convert the GUID prefix to a printable hex string. + /// @return Colon-separated hexadecimal representation of the prefix. + std::string to_string() const; + }; + + /// @brief 4-byte entity identifier within a participant. + struct EntityId { + std::array value{}; ///< Raw 4-byte entity identifier value. + + /// @brief Compare two entity identifiers for equality. + /// @param other Entity identifier to compare against. + /// @return True if both entity identifiers contain identical bytes. + bool operator==(const EntityId &other) const = default; + + /// @brief Convert the entity identifier to a printable hex string. + /// @return Colon-separated hexadecimal representation of the entity identifier. + std::string to_string() const; + }; + + /// @brief Globally unique identifier for an RTPS entity. + struct Guid { + GuidPrefix prefix{}; ///< Participant GUID prefix portion. + EntityId entity_id{}; ///< Entity identifier portion within the participant. + + /// @brief Compare two GUIDs for equality. + /// @param other GUID to compare against. + /// @return True if both GUIDs have the same prefix and entity identifier. + bool operator==(const Guid &other) const = default; + + /// @brief Convert the GUID to a printable string. + /// @return Combined printable representation of the prefix and entity identifier. + std::string to_string() const; + }; + + /// @brief RTPS sequence number wrapper. + struct SequenceNumber { + int64_t value{1}; ///< Signed 64-bit RTPS sequence number value. + }; + + /// @brief RTPS network locator for a unicast or multicast transport endpoint. + struct Locator { + /// @brief Supported locator transport kinds. + enum class Kind : int32_t { + INVALID = -1, ///< Locator is not initialized or not valid. + UDP_V4 = 1, ///< UDP over IPv4 locator. + }; + + Kind kind{Kind::INVALID}; ///< Transport kind of this locator. + uint32_t port{0}; ///< Transport port number in host byte order. + std::array address{}; ///< Raw 16-byte RTPS locator address field. + + /// @brief Build a UDPv4 locator from a dotted IPv4 address and port. + /// @param ipv4_address Dotted-decimal IPv4 address string. + /// @param port UDP port number to advertise. + /// @return A locator configured for UDPv4 with the IPv4 address stored in RTPS locator form. + static Locator udp_v4(std::string_view ipv4_address, uint16_t port); + + /// @brief Convert the locator address to a printable IPv4 string. + /// @return Dotted-decimal IPv4 address, or `0.0.0.0` if the locator is not UDPv4. + std::string address_string() const; + }; + + /// @brief RTPS message header fields. + struct Header { + ProtocolVersion protocol_version{}; ///< RTPS protocol version. + VendorId vendor_id{}; ///< Sender vendor identifier. + GuidPrefix guid_prefix{}; ///< Sender participant GUID prefix. + }; + + /// @brief Supported RTPS submessage kinds used by the current implementation. + enum class SubmessageKind : uint8_t { + PAD = 0x01, ///< Padding submessage. + ACKNACK = 0x06, ///< Reliable-reader acknowledgement submessage. + HEARTBEAT = 0x07, ///< Reliable-writer heartbeat submessage. + INFO_TS = 0x09, ///< Timestamp information submessage. + INFO_DST = 0x0e, ///< Destination GUID-prefix information submessage. + DATA = 0x15, ///< User or discovery data submessage. + }; + + /// @brief One RTPS submessage within an RTPS message. + struct Submessage { + SubmessageKind kind{SubmessageKind::PAD}; ///< Submessage kind discriminator. + uint8_t flags{0x01}; ///< Raw RTPS submessage flags byte. + std::vector + payload{}; ///< Serialized submessage payload bytes without the 4-byte submessage header. + }; + + /// @brief RTPS message consisting of a header and a sequence of submessages. + struct Message { + Header header{}; ///< RTPS message header. + std::vector submessages{}; ///< Serialized submessages carried by this RTPS message. + + /// @brief Serialize the RTPS message to bytes. + /// @return A complete RTPS message buffer ready to send on the network. + std::vector serialize() const; + + /// @brief Parse an RTPS message from bytes. + /// @param data Serialized RTPS message bytes. + /// @return A parsed message on success, or `std::nullopt` if the input is invalid. + static std::optional parse(std::span data); + }; + + /// @brief Standard RTPS UDP port mapping derived from domain and participant IDs. + struct PortMapping { + uint16_t metatraffic_multicast{0}; ///< Multicast discovery/metatraffic port. + uint16_t metatraffic_unicast{0}; ///< Unicast discovery/metatraffic port for this participant. + uint16_t user_multicast{0}; ///< Multicast user-data port. + uint16_t user_unicast{0}; ///< Unicast user-data port for this participant. + }; + + /// @brief Configuration for a locally advertised writer endpoint. + struct WriterConfig { + std::string topic_name{}; ///< Topic name advertised through SEDP. + std::string type_name{"std_msgs/msg/UInt32"}; ///< Type name advertised through SEDP. + ReliabilityKind reliability{ + ReliabilityKind::BEST_EFFORT}; ///< Reliability QoS advertised for the writer. + uint32_t entity_index{0}; ///< Local entity slot used to derive the RTPS entity ID. + }; + + /// @brief Configuration for a locally advertised reader endpoint. + struct ReaderConfig { + std::string topic_name{}; ///< Topic name advertised through SEDP. + std::string type_name{"std_msgs/msg/UInt32"}; ///< Type name advertised through SEDP. + ReliabilityKind reliability{ + ReliabilityKind::BEST_EFFORT}; ///< Reliability QoS advertised for the reader. + uint32_t entity_index{0}; ///< Local entity slot used to derive the RTPS entity ID. + std::function on_uint32_sample{ + nullptr}; ///< Callback invoked when a matching temporary UInt32 sample is received. + }; + + /// @brief Cached information about a discovered remote participant. + struct ParticipantProxy { + Guid participant_guid{}; ///< Discovered participant GUID. + GuidPrefix guid_prefix{}; ///< Discovered participant GUID prefix. + std::string name{}; ///< Remote participant name, if advertised. + std::string enclave{"/"}; ///< Remote ROS 2 enclave/user-data hint, if advertised. + std::string address{}; ///< Preferred remote IPv4 address for user traffic. + PortMapping ports{}; ///< Remote participant port mapping derived from discovery data. + uint32_t builtin_endpoints{0}; ///< Remote builtin-endpoint bitmask from SPDP. + }; + + /// @brief Cached information about a discovered remote reader or writer endpoint. + struct EndpointProxy { + Guid guid{}; ///< Discovered endpoint GUID. + Guid participant_guid{}; ///< GUID of the participant that owns this endpoint. + std::string topic_name{}; ///< Discovered topic name. + std::string type_name{"std_msgs/msg/UInt32"}; ///< Discovered type name. + ReliabilityKind reliability{ReliabilityKind::BEST_EFFORT}; ///< Advertised endpoint reliability. + bool is_reader{false}; ///< True for discovered readers, false for discovered writers. + bool expects_inline_qos{false}; ///< Whether the remote endpoint requested inline QoS. + Locator unicast_locator{}; ///< Preferred unicast locator advertised by the endpoint. + }; + + /// @brief Top-level participant configuration. + struct Config { + std::string node_name{"espp_rtps"}; ///< Local participant name advertised in discovery. + uint16_t domain_id{0}; ///< RTPS domain ID used for port derivation and discovery scope. + uint16_t participant_id{0}; ///< RTPS participant ID used for GUID and port derivation. + std::string bind_address{"0.0.0.0"}; ///< Local IPv4 address to bind sockets to. + std::string advertised_address{ + "127.0.0.1"}; ///< IPv4 address advertised to peers for unicast traffic. + std::string metatraffic_multicast_group{ + "239.255.0.1"}; ///< Multicast group used for RTPS metatraffic discovery. + Task::BaseConfig receive_task_config{ + .name = "RtpsRx", + .stack_size_bytes = 6 * 1024}; ///< Base task configuration for receive sockets. + Task::BaseConfig announce_task_config{ + .name = "RtpsAnnounce", + .stack_size_bytes = 6 * 1024}; ///< Task configuration for periodic discovery announcements. + std::chrono::milliseconds announce_period{1000}; ///< Interval between periodic SPDP/SEDP sends. + std::string enclave{"/"}; ///< User-data enclave string advertised in SPDP. + std::function on_participant_discovered{ + nullptr}; ///< Callback invoked when a remote participant is first discovered. + std::function on_endpoint_discovered{ + nullptr}; ///< Callback invoked when a remote endpoint is first discovered. + espp::Logger::Verbosity log_level{ + espp::Logger::Verbosity::INFO}; ///< Participant log verbosity. + }; + + /// @brief Construct an RTPS participant. + /// @param config Participant configuration controlling ports, addresses, discovery, and + /// callbacks. + explicit RtpsParticipant(const Config &config); + + /// @brief Destroy the participant and stop any active sockets/tasks. + ~RtpsParticipant(); + + /// @brief Start discovery sockets, user-data sockets, and periodic announcements. + /// @return True if startup succeeded, false if the participant was already started or + /// initialization failed. + bool start(); + + /// @brief Stop sockets and background tasks associated with the participant. + void stop(); + + /// @brief Check whether the participant is currently started. + /// @return True if the participant has been started and not yet stopped. + bool is_started() const; + + /// @brief Register a local writer endpoint to advertise through SEDP. + /// @param writer_config Writer configuration to add. + /// @return True after the writer has been stored. + bool add_writer(const WriterConfig &writer_config); + + /// @brief Register a local reader endpoint to advertise through SEDP. + /// @param reader_config Reader configuration to add. + /// @return True after the reader has been stored. + bool add_reader(const ReaderConfig &reader_config); + + /// @brief Get the currently discovered remote participants. + /// @return A snapshot copy of the discovered participant list. + std::vector discovered_participants() const; + + /// @brief Get the currently discovered remote writer endpoints. + /// @return A snapshot copy of the discovered writer list. + std::vector discovered_writers() const; + + /// @brief Get the currently discovered remote reader endpoints. + /// @return A snapshot copy of the discovered reader list. + std::vector discovered_readers() const; + + /// @brief Access the registered local writer configurations. + /// @return A const reference to the local writer list. + const std::vector &writers() const; + + /// @brief Access the registered local reader configurations. + /// @return A const reference to the local reader list. + const std::vector &readers() const; + + /// @brief Compute the standard RTPS UDP port mapping for this participant. + /// @return The derived metatraffic and user-data ports for the configured domain and participant + /// IDs. + PortMapping ports() const; + + /// @brief Get the local participant GUID. + /// @return GUID built from the local GUID prefix and participant entity ID. + Guid participant_guid() const; + + /// @brief Get the GUID for a local writer entity slot. + /// @param index Zero-based local writer entity index. + /// @return GUID for the derived local writer entity. + Guid writer_guid(size_t index) const; + + /// @brief Get the GUID for a local reader entity slot. + /// @param index Zero-based local reader entity index. + /// @return GUID for the derived local reader entity. + Guid reader_guid(size_t index) const; + + /// @brief Build the default participant announce message. + /// @return Serialized SPDP participant announce message. + std::vector build_announce_message() const; + + /// @brief Build the SPDP participant announcement message for this participant. + /// @return Serialized SPDP message describing the local participant. + std::vector build_spdp_announce_message() const; + + /// @brief Build an SEDP publication announcement for a local writer. + /// @param writer_config Writer configuration to serialize. + /// @return Serialized SEDP publication message for the writer. + std::vector build_sedp_publication_message(const WriterConfig &writer_config) const; + + /// @brief Build an SEDP subscription announcement for a local reader. + /// @param reader_config Reader configuration to serialize. + /// @return Serialized SEDP subscription message for the reader. + std::vector build_sedp_subscription_message(const ReaderConfig &reader_config) const; + + /// @brief Build a temporary ESPP UInt32 user-data message. + /// @param topic_name Topic name to embed in the message payload. + /// @param value UInt32 sample value to serialize. + /// @param reliability Reliability flag to encode in the temporary payload header. + /// @return Serialized RTPS DATA message containing the temporary ESPP UInt32 payload. + std::vector build_uint32_data_message(std::string_view topic_name, uint32_t value, + ReliabilityKind reliability) const; + + /// @brief Publish a temporary ESPP UInt32 sample to discovered participants. + /// @param topic_name Topic name to publish on. Must match a registered local writer. + /// @param value UInt32 sample value to send. + /// @return True if at least one send call succeeded, false otherwise. + bool publish_uint32(std::string_view topic_name, uint32_t value); + + /// @brief Serialize a UInt32 value into a standalone CDR payload. + /// @param value Value to serialize. + /// @return Encapsulated little-endian CDR payload containing the value. + static std::vector serialize_uint32_cdr(uint32_t value); + + /// @brief Parse a standalone CDR payload containing a UInt32 value. + /// @param data Encapsulated CDR payload bytes. + /// @return Parsed UInt32 value on success, or `std::nullopt` if the payload is invalid. + static std::optional deserialize_uint32_cdr(std::span data); + + /// @brief Compute the standard RTPS UDP port mapping for a domain/participant pair. + /// @param domain_id RTPS domain ID. + /// @param participant_id RTPS participant ID. + /// @return Derived RTPS metatraffic and user-data ports. + static PortMapping compute_port_mapping(uint16_t domain_id, uint16_t participant_id); + +private: + bool handle_metatraffic_message(std::vector &data, const Socket::Info &sender); + bool handle_user_message(std::vector &data, const Socket::Info &sender); + bool send_spdp_announce_now(); + bool send_sedp_announcements_to(const ParticipantProxy &participant); + bool send_discovery_now(); + ParticipantProxy make_local_participant_proxy() const; + + Config config_; + GuidPrefix guid_prefix_{}; + std::atomic_bool started_{false}; + + std::unique_ptr metatraffic_multicast_receiver_; + std::unique_ptr metatraffic_unicast_receiver_; + std::unique_ptr user_unicast_receiver_; + std::unique_ptr announce_task_; + + mutable std::mutex mutex_; + std::vector writers_; + std::vector readers_; + std::vector discovered_participants_; + std::vector discovered_writers_; + std::vector discovered_readers_; +}; +} // namespace espp diff --git a/components/rtps/src/rtps.cpp b/components/rtps/src/rtps.cpp new file mode 100644 index 000000000..df526118c --- /dev/null +++ b/components/rtps/src/rtps.cpp @@ -0,0 +1,1413 @@ +#include "rtps.hpp" + +#include +#include +#include +#include +#include +#include + +#include "cdr.hpp" + +namespace { +constexpr std::array kRtpsMagic{'R', 'T', 'P', 'S'}; +constexpr std::array kUserDataMagic{'E', 'S', 'P', 'P', 'D', 'A', 'T', 'A'}; +constexpr uint8_t kUserDataVersion = 1; + +constexpr uint16_t kPortBase = 7400; +constexpr uint16_t kDomainGain = 250; +constexpr uint16_t kParticipantGain = 2; +constexpr uint16_t kMetatrafficMulticastOffset = 0; +constexpr uint16_t kMetatrafficUnicastOffset = 10; +constexpr uint16_t kUserMulticastOffset = 1; +constexpr uint16_t kUserUnicastOffset = 11; + +constexpr uint8_t kSubmessageFlagLittleEndian = 0x01; +constexpr uint8_t kSubmessageFlagInlineQos = 0x02; +constexpr uint8_t kSubmessageFlagData = 0x04; +constexpr uint16_t kDataSubmessageOctetsToInlineQos = 16; + +constexpr uint32_t kBuiltinEndpointParticipantAnnouncer = 1u << 0; +constexpr uint32_t kBuiltinEndpointParticipantDetector = 1u << 1; +constexpr uint32_t kBuiltinEndpointPublicationAnnouncer = 1u << 2; +constexpr uint32_t kBuiltinEndpointPublicationDetector = 1u << 3; +constexpr uint32_t kBuiltinEndpointSubscriptionAnnouncer = 1u << 4; +constexpr uint32_t kBuiltinEndpointSubscriptionDetector = 1u << 5; +constexpr uint32_t kBuiltinEndpointParticipantMessageWriter = 1u << 10; +constexpr uint32_t kBuiltinEndpointParticipantMessageReader = 1u << 11; +constexpr uint32_t kBuiltinEndpointSet = + kBuiltinEndpointParticipantAnnouncer | kBuiltinEndpointParticipantDetector | + kBuiltinEndpointPublicationAnnouncer | kBuiltinEndpointPublicationDetector | + kBuiltinEndpointSubscriptionAnnouncer | kBuiltinEndpointSubscriptionDetector | + kBuiltinEndpointParticipantMessageWriter | kBuiltinEndpointParticipantMessageReader; + +constexpr std::array kEntityIdUnknown{{0x00, 0x00, 0x00, 0x00}}; +constexpr std::array kParticipantEntityId{{0x00, 0x00, 0x01, 0xc1}}; +constexpr std::array kSpdpWriterEntityId{{0x00, 0x01, 0x00, 0xc2}}; +constexpr std::array kSpdpReaderEntityId{{0x00, 0x01, 0x00, 0xc7}}; +constexpr std::array kSedpPublicationsWriterEntityId{{0x00, 0x00, 0x03, 0xc2}}; +constexpr std::array kSedpPublicationsReaderEntityId{{0x00, 0x00, 0x03, 0xc7}}; +constexpr std::array kSedpSubscriptionsWriterEntityId{{0x00, 0x00, 0x04, 0xc2}}; +constexpr std::array kSedpSubscriptionsReaderEntityId{{0x00, 0x00, 0x04, 0xc7}}; +constexpr uint8_t kUserWriterNoKeyKind = 0x03; +constexpr uint8_t kUserReaderNoKeyKind = 0x04; + +constexpr uint32_t kHistoryKeepLast = 0; +constexpr uint32_t kReliabilityBestEffort = 1; +constexpr uint32_t kReliabilityReliable = 2; +constexpr uint32_t kDurabilityVolatile = 0; +constexpr uint32_t kLivelinessAutomatic = 0; +constexpr int32_t kDefaultLeaseDurationSeconds = 20; +constexpr uint32_t kDefaultLeaseDurationNanoseconds = 0; +constexpr int32_t kDefaultMaxBlockingSeconds = 0; +constexpr uint32_t kDefaultMaxBlockingNanoseconds = 100000000; +constexpr uint32_t kUInt32SerializedSize = 8; + +enum class ParameterId : uint16_t { + PID_SENTINEL = 0x0001, + PID_PARTICIPANT_LEASE_DURATION = 0x0002, + PID_TOPIC_NAME = 0x0005, + PID_TYPE_NAME = 0x0007, + PID_DOMAIN_ID = 0x000f, + PID_PROTOCOL_VERSION = 0x0015, + PID_VENDORID = 0x0016, + PID_DURABILITY = 0x001d, + PID_RELIABILITY = 0x001a, + PID_LIVELINESS = 0x001b, + PID_USER_DATA = 0x002c, + PID_UNICAST_LOCATOR = 0x002f, + PID_DEFAULT_UNICAST_LOCATOR = 0x0031, + PID_METATRAFFIC_UNICAST_LOCATOR = 0x0032, + PID_METATRAFFIC_MULTICAST_LOCATOR = 0x0033, + PID_MULTICAST_LOCATOR = 0x0030, + PID_EXPECTS_INLINE_QOS = 0x0043, + PID_DEFAULT_MULTICAST_LOCATOR = 0x0048, + PID_PARTICIPANT_GUID = 0x0050, + PID_BUILTIN_ENDPOINT_SET = 0x0058, + PID_ENDPOINT_GUID = 0x005a, + PID_TYPE_MAX_SIZE_SERIALIZED = 0x0060, + PID_ENTITY_NAME = 0x0062, + PID_KEY_HASH = 0x0070, + PID_HISTORY = 0x0040, +}; + +class ByteWriter { +public: + void append_bytes(std::span bytes) { + data_.insert(data_.end(), bytes.begin(), bytes.end()); + } + + template void append_bytes(const std::array &bytes) { + data_.insert(data_.end(), bytes.begin(), bytes.end()); + } + + template void append_chars(const std::array &bytes) { + data_.insert(data_.end(), bytes.begin(), bytes.end()); + } + + void append_u8(uint8_t value) { data_.push_back(value); } + + void append_u16_le(uint16_t value) { + data_.push_back(static_cast(value & 0xff)); + data_.push_back(static_cast((value >> 8) & 0xff)); + } + + void append_u16_be(uint16_t value) { + data_.push_back(static_cast((value >> 8) & 0xff)); + data_.push_back(static_cast(value & 0xff)); + } + + void append_u32_le(uint32_t value) { + for (int i = 0; i < 4; i++) { + data_.push_back(static_cast((value >> (8 * i)) & 0xff)); + } + } + + void append_i32_le(int32_t value) { append_u32_le(static_cast(value)); } + + void append_u32_be(uint32_t value) { + for (int i = 3; i >= 0; i--) { + data_.push_back(static_cast((value >> (8 * i)) & 0xff)); + } + } + + void append_sequence_number_le(int64_t value) { + auto high = static_cast(value >> 32); + auto low = static_cast(value & 0xffffffffu); + append_i32_le(high); + append_u32_le(low); + } + + size_t size() const { return data_.size(); } + + void align(size_t alignment) { + while (data_.size() % alignment != 0) { + data_.push_back(0); + } + } + + std::vector take() { return std::move(data_); } + +private: + std::vector data_; +}; + +class ByteReader { +public: + explicit ByteReader(std::span data) + : data_(data) {} + + bool read_u8(uint8_t &value) { + if (remaining() < 1) { + return false; + } + value = data_[offset_++]; + return true; + } + + bool read_u16_le(uint16_t &value) { + if (remaining() < 2) { + return false; + } + value = static_cast(data_[offset_]) | + static_cast(static_cast(data_[offset_ + 1]) << 8); + offset_ += 2; + return true; + } + + bool read_u32_le(uint32_t &value) { + if (remaining() < 4) { + return false; + } + value = static_cast(data_[offset_]) | + (static_cast(data_[offset_ + 1]) << 8) | + (static_cast(data_[offset_ + 2]) << 16) | + (static_cast(data_[offset_ + 3]) << 24); + offset_ += 4; + return true; + } + + bool read_i32_le(int32_t &value) { + uint32_t unsigned_value = 0; + if (!read_u32_le(unsigned_value)) { + return false; + } + value = static_cast(unsigned_value); + return true; + } + + bool read_u32_be(uint32_t &value) { + if (remaining() < 4) { + return false; + } + value = (static_cast(data_[offset_]) << 24) | + (static_cast(data_[offset_ + 1]) << 16) | + (static_cast(data_[offset_ + 2]) << 8) | + static_cast(data_[offset_ + 3]); + offset_ += 4; + return true; + } + + bool read_sequence_number_le(int64_t &value) { + int32_t high = 0; + uint32_t low = 0; + if (!read_i32_le(high) || !read_u32_le(low)) { + return false; + } + value = (static_cast(high) << 32) | low; + return true; + } + + bool read_bytes(std::span destination) { + if (remaining() < destination.size()) { + return false; + } + std::memcpy(destination.data(), data_.data() + offset_, destination.size()); + offset_ += destination.size(); + return true; + } + + std::span read_span(size_t length) { + if (remaining() < length) { + return {}; + } + auto span = data_.subspan(offset_, length); + offset_ += length; + return span; + } + + size_t remaining() const { return data_.size() - offset_; } + +private: + std::span data_; + size_t offset_{0}; +}; + +struct ParameterView { + ParameterId id{ParameterId::PID_SENTINEL}; + std::span value{}; +}; + +struct DataSubmessageView { + espp::RtpsParticipant::EntityId reader_id{}; + espp::RtpsParticipant::EntityId writer_id{}; + int64_t writer_sn{0}; + std::span serialized_payload{}; + bool inline_qos_present{false}; + bool data_present{false}; +}; + +std::string hex_string(std::span bytes) { + std::ostringstream stream; + stream << std::hex << std::setfill('0'); + for (size_t i = 0; i < bytes.size(); i++) { + if (i != 0) { + stream << ':'; + } + stream << std::setw(2) << static_cast(bytes[i]); + } + return stream.str(); +} + +bool parse_ipv4(std::string_view address, std::array &octets) { + std::array parsed{}; + size_t part_index = 0; + size_t cursor = 0; + while (cursor < address.size() && part_index < parsed.size()) { + auto next = address.find('.', cursor); + if (next == std::string_view::npos) { + next = address.size(); + } + if (next == cursor) { + return false; + } + unsigned value = 0; + for (size_t i = cursor; i < next; i++) { + if (!std::isdigit(static_cast(address[i]))) { + return false; + } + value = value * 10 + static_cast(address[i] - '0'); + if (value > 255) { + return false; + } + } + parsed[part_index++] = static_cast(value); + cursor = next + 1; + } + if (part_index != parsed.size() || cursor < address.size()) { + return false; + } + octets = parsed; + return true; +} + +void append_string(ByteWriter &writer, std::string_view text) { + writer.append_u16_le(static_cast(text.size())); + writer.append_bytes( + std::span{reinterpret_cast(text.data()), text.size()}); +} + +std::optional read_string(ByteReader &reader) { + uint16_t length = 0; + if (!reader.read_u16_le(length)) { + return std::nullopt; + } + auto span = reader.read_span(length); + if (span.size() != length) { + return std::nullopt; + } + return std::string(reinterpret_cast(span.data()), span.size()); +} + +void append_parameter_header(ByteWriter &writer, ParameterId id, uint16_t length) { + writer.append_u16_le(static_cast(id)); + writer.append_u16_le(length); +} + +void append_parameter_guid(ByteWriter &writer, ParameterId id, + const espp::RtpsParticipant::Guid &guid) { + append_parameter_header(writer, id, 16); + writer.append_bytes(guid.prefix.value); + writer.append_bytes(guid.entity_id.value); +} + +void append_parameter_protocol_version(ByteWriter &writer, + const espp::RtpsParticipant::ProtocolVersion &version) { + append_parameter_header(writer, ParameterId::PID_PROTOCOL_VERSION, 4); + writer.append_u8(version.major); + writer.append_u8(version.minor); + writer.append_u8(0); + writer.append_u8(0); +} + +void append_parameter_vendor_id(ByteWriter &writer, + const espp::RtpsParticipant::VendorId &vendor_id) { + append_parameter_header(writer, ParameterId::PID_VENDORID, 4); + writer.append_bytes(vendor_id.value); + writer.append_u8(0); + writer.append_u8(0); +} + +void append_parameter_u32(ByteWriter &writer, ParameterId id, uint32_t value) { + append_parameter_header(writer, id, 4); + writer.append_u32_le(value); +} + +void append_parameter_bool(ByteWriter &writer, ParameterId id, bool value) { + append_parameter_header(writer, id, 4); + writer.append_u8(value ? 1 : 0); + writer.append_u8(0); + writer.append_u8(0); + writer.append_u8(0); +} + +void append_parameter_duration(ByteWriter &writer, ParameterId id, int32_t seconds, + uint32_t nanoseconds) { + append_parameter_header(writer, id, 8); + writer.append_i32_le(seconds); + writer.append_u32_le(nanoseconds); +} + +void append_parameter_locator(ByteWriter &writer, ParameterId id, + const espp::RtpsParticipant::Locator &locator) { + append_parameter_header(writer, id, 24); + writer.append_u32_be(static_cast(locator.kind)); + writer.append_u32_be(locator.port); + writer.append_bytes(locator.address); +} + +void append_parameter_string_cdr(ByteWriter &writer, ParameterId id, std::string_view text) { + uint16_t raw_length = static_cast(4 + text.size() + 1); + append_parameter_header(writer, id, raw_length); + auto cdr_writer = espp::CdrWriter::make_body_writer(espp::CdrEncapsulation::CDR_LE); + cdr_writer.write_string(text); + writer.append_bytes(cdr_writer.payload()); +} + +void append_parameter_octet_sequence(ByteWriter &writer, ParameterId id, + std::span bytes) { + uint16_t raw_length = static_cast(4 + bytes.size()); + append_parameter_header(writer, id, raw_length); + auto cdr_writer = espp::CdrWriter::make_body_writer(espp::CdrEncapsulation::CDR_LE); + cdr_writer.write(static_cast(bytes.size())); + cdr_writer.write_bytes(bytes); + cdr_writer.align(4); + writer.append_bytes(cdr_writer.payload()); +} + +void append_parameter_reliability(ByteWriter &writer, + espp::RtpsParticipant::ReliabilityKind reliability) { + append_parameter_header(writer, ParameterId::PID_RELIABILITY, 12); + writer.append_u32_le(reliability == espp::RtpsParticipant::ReliabilityKind::RELIABLE + ? kReliabilityReliable + : kReliabilityBestEffort); + writer.append_i32_le(kDefaultMaxBlockingSeconds); + writer.append_u32_le(kDefaultMaxBlockingNanoseconds); +} + +void append_parameter_durability(ByteWriter &writer) { + append_parameter_header(writer, ParameterId::PID_DURABILITY, 4); + writer.append_u32_le(kDurabilityVolatile); +} + +void append_parameter_liveliness(ByteWriter &writer) { + append_parameter_header(writer, ParameterId::PID_LIVELINESS, 12); + writer.append_u32_le(kLivelinessAutomatic); + writer.append_i32_le(kDefaultLeaseDurationSeconds); + writer.append_u32_le(kDefaultLeaseDurationNanoseconds); +} + +void append_parameter_history(ByteWriter &writer) { + append_parameter_header(writer, ParameterId::PID_HISTORY, 8); + writer.append_u32_le(kHistoryKeepLast); + writer.append_u32_le(1); +} + +void append_parameter_key_hash(ByteWriter &writer, const espp::RtpsParticipant::Guid &guid) { + append_parameter_header(writer, ParameterId::PID_KEY_HASH, 16); + writer.append_bytes(guid.prefix.value); + writer.append_bytes(guid.entity_id.value); +} + +void append_parameter_sentinel(ByteWriter &writer) { + append_parameter_header(writer, ParameterId::PID_SENTINEL, 0); +} + +std::vector parse_parameter_list(std::span payload) { + std::vector parameters; + espp::CdrReader cdr_reader(payload); + if (!cdr_reader.valid() || cdr_reader.encapsulation() != espp::CdrEncapsulation::PL_CDR_LE) { + return parameters; + } + + ByteReader reader(cdr_reader.payload()); + while (reader.remaining() >= 4) { + uint16_t pid = 0; + uint16_t length = 0; + if (!reader.read_u16_le(pid) || !reader.read_u16_le(length)) { + return {}; + } + if (pid == static_cast(ParameterId::PID_SENTINEL)) { + break; + } + auto value = reader.read_span(length); + if (value.size() != length) { + return {}; + } + parameters.push_back({.id = static_cast(pid), .value = value}); + auto padding = (4 - (length % 4)) & 0x3; + if (padding > 0 && reader.read_span(padding).size() != padding) { + return {}; + } + } + return parameters; +} + +std::optional find_parameter(std::span parameters, + ParameterId id) { + auto iterator = std::find_if(parameters.begin(), parameters.end(), + [id](const auto ¶meter) { return parameter.id == id; }); + if (iterator == parameters.end()) { + return std::nullopt; + } + return *iterator; +} + +std::optional parse_guid(std::span value) { + if (value.size() != 16) { + return std::nullopt; + } + espp::RtpsParticipant::Guid guid; + std::memcpy(guid.prefix.value.data(), value.data(), guid.prefix.value.size()); + std::memcpy(guid.entity_id.value.data(), value.data() + guid.prefix.value.size(), + guid.entity_id.value.size()); + return guid; +} + +std::optional parse_u32_le(std::span value) { + ByteReader reader(value); + uint32_t parsed = 0; + if (!reader.read_u32_le(parsed)) { + return std::nullopt; + } + return parsed; +} + +std::optional parse_bool(std::span value) { + if (value.size() < 1) { + return std::nullopt; + } + return value[0] != 0; +} + +std::optional parse_cdr_string(std::span value) { + auto reader = espp::CdrReader::make_body_reader(value, espp::CdrEncapsulation::CDR_LE); + if (!reader.valid()) { + return std::nullopt; + } + std::string text; + if (!reader.read_string(text)) { + return std::nullopt; + } + return text; +} + +std::optional> parse_octet_sequence(std::span value) { + auto reader = espp::CdrReader::make_body_reader(value, espp::CdrEncapsulation::CDR_LE); + if (!reader.valid()) { + return std::nullopt; + } + uint32_t length = 0; + if (!reader.read(length)) { + return std::nullopt; + } + std::vector bytes; + if (!reader.read_bytes(bytes, length)) { + return std::nullopt; + } + return bytes; +} + +std::optional parse_locator(std::span value) { + if (value.size() != 24) { + return std::nullopt; + } + ByteReader reader(value); + uint32_t kind = 0; + uint32_t port = 0; + espp::RtpsParticipant::Locator locator; + if (!reader.read_u32_be(kind) || !reader.read_u32_be(port) || + !reader.read_bytes(std::span{locator.address.data(), locator.address.size()})) { + return std::nullopt; + } + locator.kind = static_cast(static_cast(kind)); + locator.port = port; + return locator; +} + +std::optional +parse_reliability(std::span value) { + auto maybe_kind = parse_u32_le(value); + if (!maybe_kind) { + return std::nullopt; + } + if (*maybe_kind == kReliabilityReliable) { + return espp::RtpsParticipant::ReliabilityKind::RELIABLE; + } + return espp::RtpsParticipant::ReliabilityKind::BEST_EFFORT; +} + +std::string extract_enclave(std::span user_data_bytes) { + std::string text(reinterpret_cast(user_data_bytes.data()), user_data_bytes.size()); + std::string key = "enclave="; + auto position = text.find(key); + if (position == std::string::npos) { + return "/"; + } + position += key.size(); + auto end = text.find(';', position); + if (end == std::string::npos) { + end = text.size(); + } + return text.substr(position, end - position); +} + +std::array entity_id_for_index(uint32_t entity_index, uint8_t kind) { + return {0x00, 0x00, static_cast(0x10 + entity_index), kind}; +} + +bool is_same_guid_prefix(const espp::RtpsParticipant::Guid &guid, + const espp::RtpsParticipant::GuidPrefix &prefix) { + return guid.prefix == prefix; +} + +DataSubmessageView parse_data_submessage(const espp::RtpsParticipant::Submessage &submessage, + bool &ok) { + DataSubmessageView view; + ok = false; + if (submessage.kind != espp::RtpsParticipant::SubmessageKind::DATA || + (submessage.flags & kSubmessageFlagData) == 0) { + return view; + } + + ByteReader reader(std::span{submessage.payload.data(), submessage.payload.size()}); + uint16_t extra_flags = 0; + uint16_t octets_to_inline_qos = 0; + if (!reader.read_u16_le(extra_flags) || !reader.read_u16_le(octets_to_inline_qos) || + !reader.read_bytes( + std::span{view.reader_id.value.data(), view.reader_id.value.size()}) || + !reader.read_bytes( + std::span{view.writer_id.value.data(), view.writer_id.value.size()}) || + !reader.read_sequence_number_le(view.writer_sn)) { + return view; + } + + view.inline_qos_present = (submessage.flags & kSubmessageFlagInlineQos) != 0; + view.data_present = true; + if (view.inline_qos_present || octets_to_inline_qos != kDataSubmessageOctetsToInlineQos) { + return view; + } + + view.serialized_payload = reader.read_span(reader.remaining()); + ok = true; + return view; +} + +std::vector build_parameter_list_payload(ByteWriter ¶meter_writer) { + auto parameter_bytes = parameter_writer.take(); + return espp::CdrWriter::encapsulate(parameter_bytes, espp::CdrEncapsulation::PL_CDR_LE); +} + +std::vector build_data_submessage_payload(const espp::RtpsParticipant::EntityId &reader_id, + const espp::RtpsParticipant::EntityId &writer_id, + int64_t sequence_number, + std::span serialized_payload) { + ByteWriter writer; + writer.append_u16_le(0); + writer.append_u16_le(kDataSubmessageOctetsToInlineQos); + writer.append_bytes(reader_id.value); + writer.append_bytes(writer_id.value); + writer.append_sequence_number_le(sequence_number); + writer.append_bytes(serialized_payload); + writer.align(4); + return writer.take(); +} + +espp::RtpsParticipant::Message build_message(const espp::RtpsParticipant::GuidPrefix &guid_prefix, + const espp::RtpsParticipant::EntityId &reader_id, + const espp::RtpsParticipant::EntityId &writer_id, + int64_t sequence_number, + std::span serialized_payload) { + return {.header = {.guid_prefix = guid_prefix}, + .submessages = {{ + .kind = espp::RtpsParticipant::SubmessageKind::DATA, + .flags = static_cast(kSubmessageFlagLittleEndian | kSubmessageFlagData), + .payload = build_data_submessage_payload(reader_id, writer_id, sequence_number, + serialized_payload), + }}}; +} +} // namespace + +namespace espp { +std::string RtpsParticipant::GuidPrefix::to_string() const { return hex_string(value); } + +std::string RtpsParticipant::EntityId::to_string() const { return hex_string(value); } + +std::string RtpsParticipant::Guid::to_string() const { + return prefix.to_string() + '|' + entity_id.to_string(); +} + +RtpsParticipant::Locator RtpsParticipant::Locator::udp_v4(std::string_view ipv4_address, + uint16_t port) { + Locator locator; + locator.kind = Kind::UDP_V4; + locator.port = port; + std::array octets{}; + if (parse_ipv4(ipv4_address, octets)) { + locator.address[12] = octets[0]; + locator.address[13] = octets[1]; + locator.address[14] = octets[2]; + locator.address[15] = octets[3]; + } + return locator; +} + +std::string RtpsParticipant::Locator::address_string() const { + if (kind != Kind::UDP_V4) { + return "0.0.0.0"; + } + std::ostringstream stream; + stream << static_cast(address[12]) << '.' << static_cast(address[13]) << '.' + << static_cast(address[14]) << '.' << static_cast(address[15]); + return stream.str(); +} + +std::vector RtpsParticipant::Message::serialize() const { + ByteWriter writer; + writer.append_chars(kRtpsMagic); + writer.append_u8(header.protocol_version.major); + writer.append_u8(header.protocol_version.minor); + writer.append_bytes(header.vendor_id.value); + writer.append_bytes(header.guid_prefix.value); + for (const auto &submessage : submessages) { + writer.append_u8(static_cast(submessage.kind)); + writer.append_u8(submessage.flags); + writer.append_u16_le(static_cast(submessage.payload.size())); + writer.append_bytes(submessage.payload); + } + return writer.take(); +} + +std::optional +RtpsParticipant::Message::parse(std::span data) { + if (data.size() < 20 || !std::equal(kRtpsMagic.begin(), kRtpsMagic.end(), data.begin())) { + return std::nullopt; + } + + ByteReader reader(data.subspan(4)); + Message message; + if (!reader.read_u8(message.header.protocol_version.major) || + !reader.read_u8(message.header.protocol_version.minor) || + !reader.read_bytes(std::span{message.header.vendor_id.value.data(), + message.header.vendor_id.value.size()}) || + !reader.read_bytes(std::span{message.header.guid_prefix.value.data(), + message.header.guid_prefix.value.size()})) { + return std::nullopt; + } + + while (reader.remaining() > 0) { + Submessage submessage; + uint8_t kind = 0; + uint16_t length = 0; + if (!reader.read_u8(kind) || !reader.read_u8(submessage.flags) || !reader.read_u16_le(length)) { + return std::nullopt; + } + auto payload = reader.read_span(length); + if (payload.size() != length) { + return std::nullopt; + } + submessage.kind = static_cast(kind); + submessage.payload.assign(payload.begin(), payload.end()); + message.submessages.push_back(std::move(submessage)); + } + return message; +} + +RtpsParticipant::RtpsParticipant(const Config &config) + : BaseComponent({.tag = "RtpsParticipant", .level = config.log_level}) + , config_(config) { + auto hash = std::hash{}(config_.node_name); + guid_prefix_.value[0] = config_.participant_id & 0xff; + guid_prefix_.value[1] = (config_.participant_id >> 8) & 0xff; + guid_prefix_.value[2] = config_.domain_id & 0xff; + guid_prefix_.value[3] = (config_.domain_id >> 8) & 0xff; + for (size_t i = 0; i < 8; i++) { + guid_prefix_.value[4 + i] = static_cast((hash >> (8 * i)) & 0xff); + } +} + +RtpsParticipant::~RtpsParticipant() { stop(); } + +bool RtpsParticipant::start() { + if (started_.exchange(true)) { + return false; + } + + auto port_mapping = ports(); + metatraffic_multicast_receiver_ = + std::make_unique(UdpSocket::Config{.log_level = get_log_level()}); + metatraffic_unicast_receiver_ = + std::make_unique(UdpSocket::Config{.log_level = get_log_level()}); + user_unicast_receiver_ = + std::make_unique(UdpSocket::Config{.log_level = get_log_level()}); + + auto multicast_task_config = config_.receive_task_config; + multicast_task_config.name = config_.receive_task_config.name + "_spdp_mc"; + auto multicast_receive_config = UdpSocket::ReceiveConfig{ + .port = port_mapping.metatraffic_multicast, + .buffer_size = 4096, + .is_multicast_endpoint = true, + .multicast_group = config_.metatraffic_multicast_group, + .on_receive_callback = [this](auto &data, + const auto &sender) -> std::optional> { + handle_metatraffic_message(data, sender); + return std::nullopt; + }, + }; + if (!metatraffic_multicast_receiver_->start_receiving(multicast_task_config, + multicast_receive_config)) { + logger_.error("Failed to start metatraffic multicast receiver"); + stop(); + return false; + } + + auto unicast_meta_task_config = config_.receive_task_config; + unicast_meta_task_config.name = config_.receive_task_config.name + "_meta_uc"; + auto unicast_meta_receive_config = UdpSocket::ReceiveConfig{ + .port = port_mapping.metatraffic_unicast, + .buffer_size = 4096, + .on_receive_callback = [this](auto &data, + const auto &sender) -> std::optional> { + handle_metatraffic_message(data, sender); + return std::nullopt; + }, + }; + if (!metatraffic_unicast_receiver_->start_receiving(unicast_meta_task_config, + unicast_meta_receive_config)) { + logger_.error("Failed to start metatraffic unicast receiver"); + stop(); + return false; + } + + auto user_task_config = config_.receive_task_config; + user_task_config.name = config_.receive_task_config.name + "_user_uc"; + auto user_receive_config = UdpSocket::ReceiveConfig{ + .port = port_mapping.user_unicast, + .buffer_size = 4096, + .on_receive_callback = [this](auto &data, + const auto &sender) -> std::optional> { + handle_user_message(data, sender); + return std::nullopt; + }, + }; + if (!user_unicast_receiver_->start_receiving(user_task_config, user_receive_config)) { + logger_.error("Failed to start user unicast receiver"); + stop(); + return false; + } + + announce_task_ = Task::make_unique({ + .callback = [this](std::mutex &mutex, std::condition_variable &cv, bool ¬ified) -> bool { + send_discovery_now(); + std::unique_lock lock(mutex); + auto stop_requested = + cv.wait_for(lock, config_.announce_period, [¬ified] { return notified; }); + notified = false; + return stop_requested; + }, + .task_config = config_.announce_task_config, + .log_level = get_log_level(), + }); + announce_task_->start(); + send_discovery_now(); + return true; +} + +void RtpsParticipant::stop() { + started_ = false; + if (announce_task_) { + announce_task_->stop(); + announce_task_.reset(); + } + if (metatraffic_multicast_receiver_) { + metatraffic_multicast_receiver_->stop_receiving(); + metatraffic_multicast_receiver_.reset(); + } + if (metatraffic_unicast_receiver_) { + metatraffic_unicast_receiver_->stop_receiving(); + metatraffic_unicast_receiver_.reset(); + } + if (user_unicast_receiver_) { + user_unicast_receiver_->stop_receiving(); + user_unicast_receiver_.reset(); + } +} + +bool RtpsParticipant::is_started() const { return started_.load(); } + +bool RtpsParticipant::add_writer(const WriterConfig &writer_config) { + std::lock_guard lock(mutex_); + writers_.push_back(writer_config); + return true; +} + +bool RtpsParticipant::add_reader(const ReaderConfig &reader_config) { + std::lock_guard lock(mutex_); + readers_.push_back(reader_config); + return true; +} + +std::vector RtpsParticipant::discovered_participants() const { + std::lock_guard lock(mutex_); + return discovered_participants_; +} + +std::vector RtpsParticipant::discovered_writers() const { + std::lock_guard lock(mutex_); + return discovered_writers_; +} + +std::vector RtpsParticipant::discovered_readers() const { + std::lock_guard lock(mutex_); + return discovered_readers_; +} + +const std::vector &RtpsParticipant::writers() const { + return writers_; +} + +const std::vector &RtpsParticipant::readers() const { + return readers_; +} + +RtpsParticipant::PortMapping RtpsParticipant::ports() const { + return compute_port_mapping(config_.domain_id, config_.participant_id); +} + +RtpsParticipant::Guid RtpsParticipant::participant_guid() const { + return {.prefix = guid_prefix_, .entity_id = {.value = kParticipantEntityId}}; +} + +RtpsParticipant::Guid RtpsParticipant::writer_guid(size_t index) const { + return {.prefix = guid_prefix_, + .entity_id = { + .value = entity_id_for_index(static_cast(index), kUserWriterNoKeyKind)}}; +} + +RtpsParticipant::Guid RtpsParticipant::reader_guid(size_t index) const { + return {.prefix = guid_prefix_, + .entity_id = { + .value = entity_id_for_index(static_cast(index), kUserReaderNoKeyKind)}}; +} + +std::vector RtpsParticipant::build_announce_message() const { + return build_spdp_announce_message(); +} + +std::vector RtpsParticipant::build_spdp_announce_message() const { + ByteWriter parameters; + append_parameter_protocol_version(parameters, ProtocolVersion{}); + append_parameter_vendor_id(parameters, VendorId{}); + append_parameter_u32(parameters, ParameterId::PID_DOMAIN_ID, config_.domain_id); + append_parameter_guid(parameters, ParameterId::PID_PARTICIPANT_GUID, participant_guid()); + append_parameter_locator( + parameters, ParameterId::PID_METATRAFFIC_MULTICAST_LOCATOR, + Locator::udp_v4(config_.metatraffic_multicast_group, ports().metatraffic_multicast)); + append_parameter_locator( + parameters, ParameterId::PID_METATRAFFIC_UNICAST_LOCATOR, + Locator::udp_v4(config_.advertised_address, ports().metatraffic_unicast)); + append_parameter_locator(parameters, ParameterId::PID_DEFAULT_UNICAST_LOCATOR, + Locator::udp_v4(config_.advertised_address, ports().user_unicast)); + append_parameter_locator( + parameters, ParameterId::PID_DEFAULT_MULTICAST_LOCATOR, + Locator::udp_v4(config_.metatraffic_multicast_group, ports().user_multicast)); + append_parameter_duration(parameters, ParameterId::PID_PARTICIPANT_LEASE_DURATION, + kDefaultLeaseDurationSeconds, kDefaultLeaseDurationNanoseconds); + append_parameter_u32(parameters, ParameterId::PID_BUILTIN_ENDPOINT_SET, kBuiltinEndpointSet); + std::string enclave_text = "enclave=" + config_.enclave + ";"; + append_parameter_octet_sequence( + parameters, ParameterId::PID_USER_DATA, + std::span{reinterpret_cast(enclave_text.data()), + enclave_text.size()}); + append_parameter_string_cdr(parameters, ParameterId::PID_ENTITY_NAME, config_.node_name); + append_parameter_sentinel(parameters); + + auto payload = build_parameter_list_payload(parameters); + return build_message(guid_prefix_, {.value = kEntityIdUnknown}, {.value = kSpdpWriterEntityId}, 1, + payload) + .serialize(); +} + +std::vector +RtpsParticipant::build_sedp_publication_message(const WriterConfig &writer_config) const { + ByteWriter parameters; + auto guid = writer_guid(writer_config.entity_index); + append_parameter_guid(parameters, ParameterId::PID_ENDPOINT_GUID, guid); + append_parameter_locator(parameters, ParameterId::PID_UNICAST_LOCATOR, + Locator::udp_v4(config_.advertised_address, ports().user_unicast)); + append_parameter_guid(parameters, ParameterId::PID_PARTICIPANT_GUID, participant_guid()); + append_parameter_string_cdr(parameters, ParameterId::PID_TOPIC_NAME, writer_config.topic_name); + append_parameter_string_cdr(parameters, ParameterId::PID_TYPE_NAME, writer_config.type_name); + append_parameter_key_hash(parameters, guid); + append_parameter_u32(parameters, ParameterId::PID_TYPE_MAX_SIZE_SERIALIZED, + kUInt32SerializedSize); + append_parameter_protocol_version(parameters, ProtocolVersion{}); + append_parameter_vendor_id(parameters, VendorId{}); + append_parameter_durability(parameters); + append_parameter_liveliness(parameters); + append_parameter_reliability(parameters, writer_config.reliability); + append_parameter_history(parameters); + append_parameter_sentinel(parameters); + + auto payload = build_parameter_list_payload(parameters); + return build_message(guid_prefix_, {.value = kSedpPublicationsReaderEntityId}, + {.value = kSedpPublicationsWriterEntityId}, + static_cast(writer_config.entity_index + 1), payload) + .serialize(); +} + +std::vector +RtpsParticipant::build_sedp_subscription_message(const ReaderConfig &reader_config) const { + ByteWriter parameters; + auto guid = reader_guid(reader_config.entity_index); + append_parameter_guid(parameters, ParameterId::PID_ENDPOINT_GUID, guid); + append_parameter_locator(parameters, ParameterId::PID_UNICAST_LOCATOR, + Locator::udp_v4(config_.advertised_address, ports().user_unicast)); + append_parameter_bool(parameters, ParameterId::PID_EXPECTS_INLINE_QOS, false); + append_parameter_guid(parameters, ParameterId::PID_PARTICIPANT_GUID, participant_guid()); + append_parameter_string_cdr(parameters, ParameterId::PID_TOPIC_NAME, reader_config.topic_name); + append_parameter_string_cdr(parameters, ParameterId::PID_TYPE_NAME, reader_config.type_name); + append_parameter_key_hash(parameters, guid); + append_parameter_protocol_version(parameters, ProtocolVersion{}); + append_parameter_vendor_id(parameters, VendorId{}); + append_parameter_durability(parameters); + append_parameter_liveliness(parameters); + append_parameter_reliability(parameters, reader_config.reliability); + append_parameter_history(parameters); + append_parameter_sentinel(parameters); + + auto payload = build_parameter_list_payload(parameters); + return build_message(guid_prefix_, {.value = kSedpSubscriptionsReaderEntityId}, + {.value = kSedpSubscriptionsWriterEntityId}, + static_cast(reader_config.entity_index + 1), payload) + .serialize(); +} + +std::vector RtpsParticipant::build_uint32_data_message(std::string_view topic_name, + uint32_t value, + ReliabilityKind reliability) const { + ByteWriter payload_writer; + payload_writer.append_chars(kUserDataMagic); + payload_writer.append_u8(kUserDataVersion); + payload_writer.append_u8(static_cast(reliability)); + append_string(payload_writer, topic_name); + auto cdr = serialize_uint32_cdr(value); + payload_writer.append_u16_le(static_cast(cdr.size())); + payload_writer.append_bytes(cdr); + + auto guid = writers_.empty() ? writer_guid(0) : writer_guid(writers_.front().entity_index); + return build_message(guid_prefix_, {.value = kEntityIdUnknown}, guid.entity_id, 1, + payload_writer.take()) + .serialize(); +} + +bool RtpsParticipant::publish_uint32(std::string_view topic_name, uint32_t value) { + WriterConfig writer_config; + { + std::lock_guard lock(mutex_); + auto iterator = + std::find_if(writers_.begin(), writers_.end(), + [topic_name](const auto &writer) { return writer.topic_name == topic_name; }); + if (iterator == writers_.end()) { + logger_.warn("No writer registered for topic '{}'", topic_name); + return false; + } + writer_config = *iterator; + } + + if (writer_config.reliability == ReliabilityKind::RELIABLE) { + logger_.warn("Reliable user-data retransmission is not implemented yet; sending best-effort"); + } + + auto payload = build_uint32_data_message(topic_name, value, writer_config.reliability); + auto participants = discovered_participants(); + if (participants.empty()) { + logger_.warn("No discovered participants available for topic '{}'", topic_name); + return false; + } + + if (!user_unicast_receiver_) { + return false; + } + + bool sent = false; + for (const auto &participant : participants) { + auto send_config = UdpSocket::SendConfig{ + .ip_address = participant.address, + .port = participant.ports.user_unicast, + }; + sent = user_unicast_receiver_->send(payload, send_config) || sent; + } + return sent; +} + +std::vector RtpsParticipant::serialize_uint32_cdr(uint32_t value) { + espp::CdrWriter writer({ + .encapsulation = espp::CdrEncapsulation::CDR_LE, + .include_encapsulation = true, + }); + writer.write(value); + return writer.take_buffer(); +} + +std::optional RtpsParticipant::deserialize_uint32_cdr(std::span data) { + espp::CdrReader reader(data); + if (!reader.valid()) { + return std::nullopt; + } + uint32_t value = 0; + if (!reader.read(value)) { + return std::nullopt; + } + return value; +} + +RtpsParticipant::PortMapping RtpsParticipant::compute_port_mapping(uint16_t domain_id, + uint16_t participant_id) { + auto base = static_cast(kPortBase) + static_cast(kDomainGain) * domain_id; + auto participant_offset = static_cast(kParticipantGain) * participant_id; + return {.metatraffic_multicast = static_cast(base + kMetatrafficMulticastOffset), + .metatraffic_unicast = + static_cast(base + kMetatrafficUnicastOffset + participant_offset), + .user_multicast = static_cast(base + kUserMulticastOffset), + .user_unicast = static_cast(base + kUserUnicastOffset + participant_offset)}; +} + +bool RtpsParticipant::handle_metatraffic_message(std::vector &data, + const Socket::Info &sender) { + auto message = Message::parse(data); + if (!message) { + return false; + } + + for (const auto &submessage : message->submessages) { + bool valid_data = false; + auto data_view = parse_data_submessage(submessage, valid_data); + if (!valid_data) { + continue; + } + + auto parameters = parse_parameter_list(data_view.serialized_payload); + if (parameters.empty()) { + continue; + } + + if (data_view.writer_id.value == kSpdpWriterEntityId) { + auto maybe_participant_guid_parameter = + find_parameter(parameters, ParameterId::PID_PARTICIPANT_GUID); + if (!maybe_participant_guid_parameter) { + continue; + } + auto maybe_participant_guid = parse_guid(maybe_participant_guid_parameter->value); + if (!maybe_participant_guid || is_same_guid_prefix(*maybe_participant_guid, guid_prefix_)) { + continue; + } + + ParticipantProxy participant; + participant.participant_guid = *maybe_participant_guid; + participant.guid_prefix = maybe_participant_guid->prefix; + participant.address = sender.address; + + if (auto maybe_name_parameter = find_parameter(parameters, ParameterId::PID_ENTITY_NAME)) { + if (auto maybe_name = parse_cdr_string(maybe_name_parameter->value)) { + participant.name = *maybe_name; + } + } + if (auto maybe_user_data_parameter = find_parameter(parameters, ParameterId::PID_USER_DATA)) { + if (auto maybe_user_data = parse_octet_sequence(maybe_user_data_parameter->value)) { + participant.enclave = extract_enclave(*maybe_user_data); + } + } + if (auto maybe_builtin_endpoint_parameter = + find_parameter(parameters, ParameterId::PID_BUILTIN_ENDPOINT_SET)) { + if (auto maybe_builtin_endpoints = parse_u32_le(maybe_builtin_endpoint_parameter->value)) { + participant.builtin_endpoints = *maybe_builtin_endpoints; + } + } + if (auto maybe_meta_unicast_parameter = + find_parameter(parameters, ParameterId::PID_METATRAFFIC_UNICAST_LOCATOR)) { + if (auto maybe_locator = parse_locator(maybe_meta_unicast_parameter->value)) { + participant.ports.metatraffic_unicast = static_cast(maybe_locator->port); + } + } + if (auto maybe_meta_multicast_parameter = + find_parameter(parameters, ParameterId::PID_METATRAFFIC_MULTICAST_LOCATOR)) { + if (auto maybe_locator = parse_locator(maybe_meta_multicast_parameter->value)) { + participant.ports.metatraffic_multicast = static_cast(maybe_locator->port); + } + } + if (auto maybe_default_unicast_parameter = + find_parameter(parameters, ParameterId::PID_DEFAULT_UNICAST_LOCATOR)) { + if (auto maybe_locator = parse_locator(maybe_default_unicast_parameter->value)) { + participant.ports.user_unicast = static_cast(maybe_locator->port); + participant.address = maybe_locator->address_string(); + } + } + if (auto maybe_default_multicast_parameter = + find_parameter(parameters, ParameterId::PID_DEFAULT_MULTICAST_LOCATOR)) { + if (auto maybe_locator = parse_locator(maybe_default_multicast_parameter->value)) { + participant.ports.user_multicast = static_cast(maybe_locator->port); + } + } + + std::function callback; + bool is_new_participant = false; + { + std::lock_guard lock(mutex_); + auto iterator = + std::find_if(discovered_participants_.begin(), discovered_participants_.end(), + [&participant](const auto &candidate) { + return candidate.participant_guid == participant.participant_guid; + }); + if (iterator == discovered_participants_.end()) { + discovered_participants_.push_back(participant); + is_new_participant = true; + } else { + *iterator = participant; + } + callback = config_.on_participant_discovered; + } + + logger_.info("SPDP discovered participant '{}' at {} (meta {}, user {})", + participant.name.empty() ? participant.guid_prefix.to_string() + : participant.name, + participant.address, participant.ports.metatraffic_unicast, + participant.ports.user_unicast); + if (is_new_participant) { + send_sedp_announcements_to(participant); + if (callback) { + callback(participant); + } + } + continue; + } + + bool is_reader = false; + if (data_view.writer_id.value == kSedpPublicationsWriterEntityId) { + is_reader = false; + } else if (data_view.writer_id.value == kSedpSubscriptionsWriterEntityId) { + is_reader = true; + } else { + continue; + } + + auto maybe_endpoint_guid_parameter = find_parameter(parameters, ParameterId::PID_ENDPOINT_GUID); + if (!maybe_endpoint_guid_parameter) { + continue; + } + auto maybe_endpoint_guid = parse_guid(maybe_endpoint_guid_parameter->value); + if (!maybe_endpoint_guid || is_same_guid_prefix(*maybe_endpoint_guid, guid_prefix_)) { + continue; + } + + EndpointProxy endpoint; + endpoint.guid = *maybe_endpoint_guid; + endpoint.is_reader = is_reader; + + if (auto maybe_participant_guid_parameter = + find_parameter(parameters, ParameterId::PID_PARTICIPANT_GUID)) { + if (auto maybe_participant_guid = parse_guid(maybe_participant_guid_parameter->value)) { + endpoint.participant_guid = *maybe_participant_guid; + } + } + if (endpoint.participant_guid.entity_id.value == std::array{}) { + endpoint.participant_guid = {.prefix = endpoint.guid.prefix, + .entity_id = {.value = kParticipantEntityId}}; + } + if (auto maybe_topic_name_parameter = find_parameter(parameters, ParameterId::PID_TOPIC_NAME)) { + if (auto maybe_topic_name = parse_cdr_string(maybe_topic_name_parameter->value)) { + endpoint.topic_name = *maybe_topic_name; + } + } + if (auto maybe_type_name_parameter = find_parameter(parameters, ParameterId::PID_TYPE_NAME)) { + if (auto maybe_type_name = parse_cdr_string(maybe_type_name_parameter->value)) { + endpoint.type_name = *maybe_type_name; + } + } + if (auto maybe_locator_parameter = + find_parameter(parameters, ParameterId::PID_UNICAST_LOCATOR)) { + if (auto maybe_locator = parse_locator(maybe_locator_parameter->value)) { + endpoint.unicast_locator = *maybe_locator; + } + } + if (auto maybe_reliability_parameter = + find_parameter(parameters, ParameterId::PID_RELIABILITY)) { + if (auto maybe_reliability = parse_reliability(maybe_reliability_parameter->value)) { + endpoint.reliability = *maybe_reliability; + } + } + if (auto maybe_inline_qos_parameter = + find_parameter(parameters, ParameterId::PID_EXPECTS_INLINE_QOS)) { + if (auto maybe_inline_qos = parse_bool(maybe_inline_qos_parameter->value)) { + endpoint.expects_inline_qos = *maybe_inline_qos; + } + } + + std::function endpoint_callback; + bool is_new_endpoint = false; + { + std::lock_guard lock(mutex_); + auto &endpoint_list = is_reader ? discovered_readers_ : discovered_writers_; + auto iterator = std::find_if( + endpoint_list.begin(), endpoint_list.end(), + [&endpoint](const auto &candidate) { return candidate.guid == endpoint.guid; }); + if (iterator == endpoint_list.end()) { + endpoint_list.push_back(endpoint); + is_new_endpoint = true; + } else { + *iterator = endpoint; + } + endpoint_callback = config_.on_endpoint_discovered; + } + + logger_.info("SEDP discovered {} '{}' [{}] from participant {}", + endpoint.is_reader ? "reader" : "writer", endpoint.topic_name, endpoint.type_name, + endpoint.participant_guid.to_string()); + if (is_new_endpoint && endpoint_callback) { + endpoint_callback(endpoint); + } + } + return false; +} + +bool RtpsParticipant::handle_user_message(std::vector &data, const Socket::Info &sender) { + auto message = Message::parse(data); + if (!message) { + return false; + } + + for (const auto &submessage : message->submessages) { + if (submessage.kind != SubmessageKind::DATA || + submessage.payload.size() < kUserDataMagic.size() + 2 || + !std::equal(kUserDataMagic.begin(), kUserDataMagic.end(), submessage.payload.begin())) { + continue; + } + + ByteReader reader( + std::span{submessage.payload.data(), submessage.payload.size()}); + std::array magic{}; + uint8_t version = 0; + uint8_t reliability = 0; + if (!reader.read_bytes(std::span{magic.data(), magic.size()}) || + !reader.read_u8(version) || !reader.read_u8(reliability)) { + continue; + } + auto topic_name = read_string(reader); + uint16_t payload_length = 0; + if (version != kUserDataVersion || !topic_name || !reader.read_u16_le(payload_length)) { + continue; + } + auto payload = reader.read_span(payload_length); + auto maybe_value = deserialize_uint32_cdr(payload); + if (!maybe_value) { + continue; + } + + if (static_cast(reliability) == ReliabilityKind::RELIABLE) { + logger_.warn( + "Received reliable topic '{}' from {}, but ACKNACK/HEARTBEAT is not implemented yet", + *topic_name, sender); + } + + std::vector> callbacks; + { + std::lock_guard lock(mutex_); + for (const auto &reader_config : readers_) { + if (reader_config.topic_name == *topic_name && reader_config.on_uint32_sample) { + callbacks.push_back(reader_config.on_uint32_sample); + } + } + } + for (const auto &callback : callbacks) { + callback(*maybe_value); + } + } + return false; +} + +bool RtpsParticipant::send_spdp_announce_now() { + if (!metatraffic_unicast_receiver_) { + return false; + } + auto payload = build_spdp_announce_message(); + auto send_config = UdpSocket::SendConfig{ + .ip_address = config_.metatraffic_multicast_group, + .port = ports().metatraffic_multicast, + .is_multicast_endpoint = true, + }; + return metatraffic_unicast_receiver_->send(payload, send_config); +} + +bool RtpsParticipant::send_sedp_announcements_to(const ParticipantProxy &participant) { + if (!metatraffic_unicast_receiver_ || participant.ports.metatraffic_unicast == 0 || + participant.address.empty()) { + return false; + } + + bool sent = false; + std::vector local_writers; + std::vector local_readers; + { + std::lock_guard lock(mutex_); + local_writers = writers_; + local_readers = readers_; + } + + for (const auto &writer_config : local_writers) { + auto payload = build_sedp_publication_message(writer_config); + auto send_config = UdpSocket::SendConfig{ + .ip_address = participant.address, + .port = participant.ports.metatraffic_unicast, + }; + sent = metatraffic_unicast_receiver_->send(payload, send_config) || sent; + } + for (const auto &reader_config : local_readers) { + auto payload = build_sedp_subscription_message(reader_config); + auto send_config = UdpSocket::SendConfig{ + .ip_address = participant.address, + .port = participant.ports.metatraffic_unicast, + }; + sent = metatraffic_unicast_receiver_->send(payload, send_config) || sent; + } + return sent; +} + +bool RtpsParticipant::send_discovery_now() { + auto sent = send_spdp_announce_now(); + auto participants = discovered_participants(); + for (const auto &participant : participants) { + sent = send_sedp_announcements_to(participant) || sent; + } + return sent; +} + +RtpsParticipant::ParticipantProxy RtpsParticipant::make_local_participant_proxy() const { + return {.participant_guid = participant_guid(), + .guid_prefix = guid_prefix_, + .name = config_.node_name, + .enclave = config_.enclave, + .address = config_.advertised_address, + .ports = ports(), + .builtin_endpoints = kBuiltinEndpointSet}; +} +} // namespace espp diff --git a/doc/Doxyfile b/doc/Doxyfile index 29f2cd66c..e9e5ba911 100755 --- a/doc/Doxyfile +++ b/doc/Doxyfile @@ -90,6 +90,7 @@ EXAMPLE_PATH = \ $(PROJECT_PATH)/components/button/example/main/button_example.cpp \ $(PROJECT_PATH)/components/byte90/example/main/byte90_example.cpp \ $(PROJECT_PATH)/components/chsc6x/example/main/chsc6x_example.cpp \ + $(PROJECT_PATH)/components/cdr/example/main/cdr_example.cpp \ $(PROJECT_PATH)/components/cli/example/main/cli_example.cpp \ $(PROJECT_PATH)/components/cobs/example/main/cobs_example.cpp \ $(PROJECT_PATH)/components/color/example/main/color_example.cpp \ @@ -145,6 +146,7 @@ EXAMPLE_PATH = \ $(PROJECT_PATH)/components/qwiicnes/example/main/qwiicnes_example.cpp \ $(PROJECT_PATH)/components/remote_debug/example/main/remote_debug_example.cpp \ $(PROJECT_PATH)/components/rmt/example/main/rmt_example.cpp \ + $(PROJECT_PATH)/components/rtps/example/main/rtps_example.cpp \ $(PROJECT_PATH)/components/rtsp/example/main/rtsp_example.cpp \ $(PROJECT_PATH)/components/ping/example/main/ping_example.cpp \ $(PROJECT_PATH)/components/runqueue/example/main/runqueue_example.cpp \ @@ -214,6 +216,7 @@ INPUT = \ $(PROJECT_PATH)/components/button/include/button.hpp \ $(PROJECT_PATH)/components/byte90/include/byte90.hpp \ $(PROJECT_PATH)/components/chsc6x/include/chsc6x.hpp \ + $(PROJECT_PATH)/components/cdr/include/cdr.hpp \ $(PROJECT_PATH)/components/cli/include/cli.hpp \ $(PROJECT_PATH)/components/cli/include/line_input.hpp \ $(PROJECT_PATH)/components/cobs/include/cobs.hpp \ @@ -325,6 +328,7 @@ INPUT = \ $(PROJECT_PATH)/components/remote_debug/include/remote_debug.hpp \ $(PROJECT_PATH)/components/rmt/include/rmt.hpp \ $(PROJECT_PATH)/components/rmt/include/rmt_encoder.hpp \ + $(PROJECT_PATH)/components/rtps/include/rtps.hpp \ $(PROJECT_PATH)/components/rtsp/include/generic_depacketizer.hpp \ $(PROJECT_PATH)/components/rtsp/include/generic_packetizer.hpp \ $(PROJECT_PATH)/components/rtsp/include/h264_depacketizer.hpp \ diff --git a/doc/conf_common.py b/doc/conf_common.py index e9b121279..6cc7137d2 100644 --- a/doc/conf_common.py +++ b/doc/conf_common.py @@ -5,8 +5,14 @@ 'esp_docs.esp_extensions.dummy_build_system', 'esp_docs.esp_extensions.run_doxygen', 'myst_parser', + 'sphinxcontrib.mermaid', ] +mermaid_output_format = 'raw' +mermaid_d3_zoom = True +mermaid_dark_theme = 'neutral' +mermaid_light_theme = 'neutral' + exclude_paterns = ['build', '_build', 'detail'] # link roles config diff --git a/doc/en/cdr.rst b/doc/en/cdr.rst new file mode 100644 index 000000000..6e2868dd2 --- /dev/null +++ b/doc/en/cdr.rst @@ -0,0 +1,32 @@ +CDR (Common Data Representation) +******************************** + +The ``cdr`` component provides a small, standalone Common Data Representation +reader/writer utility aimed at standards-oriented protocols such as DDS/RTPS. + +This initial slice focuses on the pieces needed to start building interoperable +payloads without forcing applications to adopt DDS or RTPS as a whole: + +- CDR / PL_CDR encapsulation identifiers +- endian-aware primitive serialization helpers +- CDR alignment and padding handling +- string helpers using the standard CDR length-prefix + null terminator layout +- body helpers for CDR fields embedded inside larger protocol elements +- fixed-array helpers and zero-copy payload/span views +- primitive sequence helpers +- standalone usage outside RTPS + +Current scope: + +- useful as a reusable building block for future DDS/RTPS payload work +- suitable for direct use in other protocols that want CDR-style payloads +- not yet a full XTypes / XCDR2 implementation + +.. toctree:: + + cdr_example + +API Reference +------------- + +.. include-build-file:: inc/cdr.inc diff --git a/doc/en/cdr_example.md b/doc/en/cdr_example.md new file mode 100644 index 000000000..84b095f9d --- /dev/null +++ b/doc/en/cdr_example.md @@ -0,0 +1,2 @@ +```{include} ../../components/cdr/example/README.md +``` diff --git a/doc/en/index.rst b/doc/en/index.rst index ff9c77cca..c2472b496 100755 --- a/doc/en/index.rst +++ b/doc/en/index.rst @@ -20,6 +20,7 @@ This is the documentation for esp-idf c++ components, ESPP (`espp Participant["RtpsParticipant"] + Participant --> SPDP["SPDP participant DATA"] + Participant --> SEDP["SEDP publication/subscription DATA"] + Participant --> User["User DATA submessages"] + SPDP --> MetaMC["Metatraffic multicast"] + SEDP --> MetaUC["Metatraffic unicast"] + User --> UserUC["User unicast"] + MetaMC --> Peer["Remote participant"] + MetaUC --> Peer + UserUC --> Peer + +Discovery Flow +-------------- + +At a high level, discovery proceeds like this: + +.. mermaid:: + + sequenceDiagram + participant A as Local participant + participant MC as 239.255.0.1 + participant B as Remote participant + A->>MC: SPDP DATA(participant GUID, locators, enclave, builtin endpoints) + MC-->>B: multicast delivery + B->>MC: SPDP DATA(its participant metadata) + MC-->>A: multicast delivery + A->>B: SEDP publication DATA(topic/type/reliability) + A->>B: SEDP subscription DATA(topic/type/reliability) + B->>A: SEDP publication/subscription DATA + Note over A,B: Matching user-data traffic can then use the user-unicast ports + +When ``handle_metatraffic_message()`` receives SPDP, it parses: + +- the remote participant GUID +- participant name +- the ``enclave=...;`` entry carried in ``PID_USER_DATA`` +- built-in endpoint bitmasks +- metatraffic and user-data locators + +For SEDP it parses the endpoint GUID, topic name, type name, reliability, +inline-QoS expectation, and unicast locator, then updates the discovered reader +or writer cache. + +Ports and Channels +------------------ + +The component follows the standard UDPv4 RTPS port mapping formula: + +.. list-table:: + :header-rows: 1 + + * - Channel + - Formula + - Domain 0, participant 0 + * - Metatraffic multicast + - ``7400 + 250 * domain + 0`` + - ``7400`` + * - Metatraffic unicast + - ``7400 + 250 * domain + 10 + 2 * participant`` + - ``7410`` + * - User multicast + - ``7400 + 250 * domain + 1`` + - ``7401`` + * - User unicast + - ``7400 + 250 * domain + 11 + 2 * participant`` + - ``7411`` + +Current ESPP Scope +------------------ + +The current implementation is intentionally focused on the first +interoperability milestone: + +- RTPS message framing and parsing +- SPDP participant discovery +- SEDP publication and subscription discovery +- participant / endpoint caches and discovery callbacks +- simple CDR little-endian serialization helpers for ``std_msgs/msg/UInt32`` + +The following pieces are **not finished yet**: + +- reliable RTPS state machines such as ``HEARTBEAT`` and ``ACKNACK`` +- standards-based ROS 2 user-data writers/readers +- full QoS matching beyond the currently emitted discovery parameters + +Feature Status +-------------- + +.. list-table:: + :header-rows: 1 + + * - Feature + - Status + - Notes + * - RTPS header / DATA submessage serialize + parse + - **Implemented** + - Core message framing is present. + * - Standard UDPv4 RTPS port mapping + - **Implemented** + - Uses the DDSI-RTPS well-known port formula. + * - SPDP participant announce send/receive + - **Implemented** + - Multicast announce plus participant cache updates. + * - SEDP publication / subscription announce send/receive + - **Implemented** + - Local endpoints are announced and remote endpoints are cached. + * - Participant / endpoint discovery callbacks + - **Implemented** + - Exposed through ``on_participant_discovered`` and + ``on_endpoint_discovered``. + * - Temporary ``UInt32`` user-data path + - **Implemented** + - Uses the current ESPP-specific ``ESPPDATA`` payload, not a + standards-based DDS sample representation. + * - QoS fields emitted in discovery + - **Partial** + - Reliability, durability, liveliness, and history parameters are + advertised in SEDP. + * - QoS matching / policy enforcement + - **Not implemented** + - Remote QoS is parsed, but full writer/reader matching logic is still + missing. + * - Standards-based DDS user-data serialization + - **Not implemented** + - The current data path is a temporary ESPP scaffold for + ``std_msgs/msg/UInt32``. + * - Inline QoS handling + - **Not implemented** + - Discovery and user-data handling assume no inline QoS. + * - Reliable RTPS (``HEARTBEAT``, ``ACKNACK``, resend) + - **Not implemented** + - Reliable delivery is not interoperable yet. + * - Full ROS 2 topic interoperability + - **Not implemented** + - Discovery is the current milestone; ROS 2-compatible data + writers/readers are still pending. + +Relevant Specifications +----------------------- + +These are the primary standards and references for understanding the current +implementation and the remaining work: + +.. list-table:: + :header-rows: 1 + + * - Specification + - Why it matters here + * - `OMG DDSI-RTPS 2.3 `_ + - Primary wire-level reference for RTPS headers, DATA submessages, SPDP, + SEDP, locator encoding, GUIDs, and the UDP port mapping used by this + component. + * - `OMG DDS 1.4 `_ + - Defines the conceptual participant, reader, writer, topic, and QoS model + that RTPS discovery is advertising. + +Example +------- + +The :doc:`rtps_example` page demonstrates the current discovery scaffold by: + +- computing the RTPS ports for a participant +- building and parsing locally generated SPDP and SEDP messages +- round-tripping a ``UInt32`` value through the CDR helper functions +- registering best-effort and reliable topic endpoints in the participant API + +.. toctree:: + + rtps_example + +API Reference +------------- + +.. include-build-file:: inc/rtps.inc diff --git a/doc/en/rtps_example.md b/doc/en/rtps_example.md new file mode 100644 index 000000000..d213860af --- /dev/null +++ b/doc/en/rtps_example.md @@ -0,0 +1,2 @@ +```{include} ../../components/rtps/example/README.md +``` diff --git a/doc/en/rtsp.rst b/doc/en/rtsp.rst index 3de33b8d6..6e0cb43ec 100644 --- a/doc/en/rtsp.rst +++ b/doc/en/rtsp.rst @@ -7,6 +7,84 @@ an extensible packetizer/depacketizer architecture. The component handles RTP packet splitting and reassembly; encoding and decoding of media data is handled externally by the application. +How RTSP Works +-------------- + +The component uses a split control-plane / media-plane design: + +- **RTSP over TCP** handles session control such as ``OPTIONS``, ``DESCRIBE``, + ``SETUP``, ``PLAY``, ``PAUSE``, and ``TEARDOWN``. +- **SDP** returned from ``DESCRIBE`` tells the client what tracks exist, how + they are encoded, and which per-track control URLs must be used for + ``SETUP``. +- **RTP/UDP** carries encoded media packets after playback starts. +- **RTCP/UDP** sockets are created alongside RTP sockets, but the current ESPP + implementation keeps RTCP support lightweight and does not yet implement a + full control/feedback plane. + +.. mermaid:: + + sequenceDiagram + participant App as Application + participant Server as RtspServer / RtspSession + participant Client as RtspClient + App->>Server: add_track() / send_frame() + Client->>Server: OPTIONS + Server-->>Client: 200 OK + Client->>Server: DESCRIBE + Server-->>Client: SDP with session + track control paths + Client->>Server: SETUP(trackID=n, client_port=RTP-RTCP) + Server-->>Client: Session + Transport headers + Client->>Server: PLAY + Server-->>Client: 200 OK + Server-->>Client: RTP/UDP packets for each active track + Client-->>App: on_jpeg_frame() or on_frame(track_id, data) + Client->>Server: TEARDOWN + Server-->>Client: 200 OK + +In ESPP, the server generates one SDP description per session, with one +``m=...`` section and one ``a=control:.../trackID=N`` entry per registered +track. The client parses those lines during ``describe()`` and then issues +``SETUP`` once per discovered track before calling ``PLAY``. + +Packetization Pipeline +---------------------- + +The codec-specific logic is intentionally separated from the RTSP core: + +.. mermaid:: + + flowchart LR + Frame["Encoded frame bytes"] --> Packetizer["Codec packetizer"] + Packetizer --> Chunks["RTP payload chunks"] + Chunks --> Header["RtspServer adds RTP headers"] + Header --> Session["RtspSession sends UDP packets"] + Session --> ClientRtp["RtspClient RTP socket"] + ClientRtp --> Depacketizer["Codec depacketizer"] + Depacketizer --> Callback["Application callback"] + +``RtspServer::send_frame(track_id, data)`` asks the selected packetizer to split +the encoded frame into MTU-sized chunks, adds RTP headers with track-specific +SSRC and sequence numbers, and leaves the resulting packets queued for active +sessions to transmit. On the client side, ``RtspClient::handle_rtp_packet()`` +parses the RTP header, uses the payload type to find the matching depacketizer, +and emits a completed frame through either ``on_jpeg_frame`` or the generic +``on_frame(track_id, data)`` callback. + +Legacy MJPEG Compatibility +-------------------------- + +For backward compatibility, the component still preserves the older MJPEG-only +behavior: + +- ``RtspServer::send_frame(std::span)`` lazily creates a default + track 0 and uses the legacy RFC 2435-compatible MJPEG wire format. +- ``RtspClient`` automatically creates an ``MjpegDepacketizer`` when a JPEG + callback is registered and payload type 26 is discovered in SDP. + +This means older single-track MJPEG integrations can keep working while newer +multi-track applications use ``add_track()`` plus codec-specific packetizers. + RTSP Client ----------- @@ -69,6 +147,37 @@ are provided for: Custom packetizers can be created by subclassing ``RtpPacketizer`` or ``RtpDepacketizer``. +Relevant Specifications +----------------------- + +These are the main standards to keep beside the code when working on this +component: + +.. list-table:: + :header-rows: 1 + + * - Specification + - Why it matters here + * - `RFC 2326: Real Time Streaming Protocol (RTSP) `_ + - Primary control-plane reference for the RTSP/1.0 request and response + flow implemented by ``RtspClient``, ``RtspServer``, and ``RtspSession``. + * - `RFC 7826: RTSP 2.0 `_ + - Useful background for newer RTSP deployments; informative here because + the current component speaks RTSP/1.0 on the wire. + * - `RFC 3550: RTP / RTCP `_ + - Defines RTP headers, timestamps, sequence numbers, SSRC handling, and + the RTCP control protocol model used by the transport layer. + * - `RFC 4566: Session Description Protocol (SDP) `_ + - Describes the SDP ``m=``, ``a=control:``, and ``a=rtpmap:`` lines that + the server generates and the client parses during ``DESCRIBE``. + * - `RFC 3551: RTP A/V Profile `_ + - Defines common RTP payload-type and clock-rate conventions used alongside + dynamic payloads. + * - `RFC 2435: RTP Payload Format for JPEG `_ + - Reference for the MJPEG packetization and depacketization path. + * - `RFC 6184: RTP Payload Format for H.264 Video `_ + - Reference for the H.264 FU-A fragmentation and reassembly path. + Testing and Utilities --------------------- diff --git a/doc/requirements.txt b/doc/requirements.txt index 82faccbef..0d157e36c 100644 --- a/doc/requirements.txt +++ b/doc/requirements.txt @@ -4,3 +4,4 @@ cairosvg esp-docs myst-parser +sphinxcontrib-mermaid diff --git a/docker_build_docs.sh b/docker_build_docs.sh index 9d5896cac..df9b11550 100755 --- a/docker_build_docs.sh +++ b/docker_build_docs.sh @@ -3,6 +3,8 @@ export PYTHONPATH=$PYTHONPATH:/project/doc # ensure we can run git commands git config --global --add safe.directory /project +# install documentation extensions not guaranteed to be present in the base image +python -m pip install --user -r /project/doc/requirements.txt # build the docs build-docs -bs html -t esp32 -l en --project-path /project/ --source-dir /project/doc/ --doxyfile_dir /project/doc/ if ! sh /project/doc/build_latex_pdf.sh /project/_build/en/esp32/latex; then diff --git a/python/README.md b/python/README.md index d4dfa9912..429c5653f 100644 --- a/python/README.md +++ b/python/README.md @@ -47,6 +47,10 @@ This section gives a brief overview of what the scripts in this folder do. audio by default when running with a UI; use `--no-audio-playback` to disable it or `--play-audio --headless` to exercise playback without opening the video window. +- `rtps_host.py`: A pure-stdlib host-side RTPS harness for discovering an ESPP + `RtpsParticipant`, printing SPDP/SEDP metadata, and optionally publishing or + receiving the current temporary `UInt32` user-data payloads without needing + Python bindings. - `cobs_demo.py`: Demonstration of ESPP COBS functionality with native Python data types. Shows ESPP encoding/decoding, cross-library compatibility with the cobs-python library, and practical usage examples. Includes design differences explanation and validation @@ -105,7 +109,9 @@ python3 .py python3 task.py # or python3 udp_client.py -``` +# or discover / test RTPS from a host machine +python3 rtps_host.py --advertised-address +``` Note: the `udp_client.py` script requires a running instance of the `udp_server.py` script. To run the server, use the following command from @@ -114,3 +120,23 @@ another terminal: ```console python3 udp_server.py ``` + +For the default ESP RTPS example, the host harness now defaults to the +**responder** side of the request/response test, so it will subscribe to +``espp/rtps_example/request`` and echo values back on +``espp/rtps_example/response``: + +```console +python3 rtps_host.py --advertised-address 192.168.1.50 +``` + +To act as the initiator instead, swap the topics and enable periodic publishing: + +```console +python3 rtps_host.py --advertised-address 192.168.1.50 \ + --subscribe-topic espp/rtps_example/response \ + --publish-topic espp/rtps_example/request \ + --publish-value 42 \ + --publish-interval 1.0 \ + --no-echo-received +``` diff --git a/python/rtps_host.py b/python/rtps_host.py new file mode 100644 index 000000000..81eef251b --- /dev/null +++ b/python/rtps_host.py @@ -0,0 +1,1022 @@ +#!/usr/bin/env python3 +"""Simple host-side RTPS test harness for the ESPP RTPS component. + +This script speaks the current ESPP RTPS discovery wire format plus the +temporary ``UInt32`` user-data payload used by ``RtpsParticipant`` today. It is +useful for: + +1. discovering an embedded ESPP RTPS participant from a PC/host, +2. inspecting SPDP/SEDP announcements, and +3. sending or receiving ``std_msgs/msg/UInt32``-style test samples over the + temporary ESPP user-data path. + +It uses only the Python standard library, so it does not require Python +bindings or a rebuilt host ``lib/`` tree. +""" + +from __future__ import annotations + +import argparse +import hashlib +import ipaddress +import select +import socket +import struct +import sys +import time +from dataclasses import dataclass +from typing import Dict, Iterable, List, Optional + + +RTPS_MAGIC = b"RTPS" +PL_CDR_LE = b"\x00\x03\x00\x00" +USER_DATA_MAGIC = b"ESPPDATA" +USER_DATA_VERSION = 1 + +PORT_BASE = 7400 +DOMAIN_GAIN = 250 +PARTICIPANT_GAIN = 2 +METATRAFFIC_MULTICAST_OFFSET = 0 +METATRAFFIC_UNICAST_OFFSET = 10 +USER_MULTICAST_OFFSET = 1 +USER_UNICAST_OFFSET = 11 + +DATA_SUBMESSAGE_KIND = 0x15 +DATA_SUBMESSAGE_FLAGS = 0x01 | 0x04 +DATA_SUBMESSAGE_OCTETS_TO_INLINE_QOS = 16 + +RELIABILITY_BEST_EFFORT = 1 +RELIABILITY_RELIABLE = 2 + +KIND_UDP_V4 = 1 +VENDOR_ID = b"\xca\xfe" + +ENTITY_ID_UNKNOWN = b"\x00\x00\x00\x00" +PARTICIPANT_ENTITY_ID = b"\x00\x00\x01\xc1" +SPDP_WRITER_ENTITY_ID = b"\x00\x01\x00\xc2" +SPDP_READER_ENTITY_ID = b"\x00\x01\x00\xc7" +SEDP_PUBLICATIONS_WRITER_ENTITY_ID = b"\x00\x00\x03\xc2" +SEDP_PUBLICATIONS_READER_ENTITY_ID = b"\x00\x00\x03\xc7" +SEDP_SUBSCRIPTIONS_WRITER_ENTITY_ID = b"\x00\x00\x04\xc2" +SEDP_SUBSCRIPTIONS_READER_ENTITY_ID = b"\x00\x00\x04\xc7" +USER_WRITER_NO_KEY_KIND = 0x03 +USER_READER_NO_KEY_KIND = 0x04 + +BUILTIN_ENDPOINT_SET = ( + (1 << 0) + | (1 << 1) + | (1 << 2) + | (1 << 3) + | (1 << 4) + | (1 << 5) + | (1 << 10) + | (1 << 11) +) + +PID_SENTINEL = 0x0001 +PID_PARTICIPANT_LEASE_DURATION = 0x0002 +PID_TOPIC_NAME = 0x0005 +PID_TYPE_NAME = 0x0007 +PID_DOMAIN_ID = 0x000F +PID_PROTOCOL_VERSION = 0x0015 +PID_VENDORID = 0x0016 +PID_RELIABILITY = 0x001A +PID_LIVELINESS = 0x001B +PID_DURABILITY = 0x001D +PID_USER_DATA = 0x002C +PID_UNICAST_LOCATOR = 0x002F +PID_DEFAULT_UNICAST_LOCATOR = 0x0031 +PID_METATRAFFIC_UNICAST_LOCATOR = 0x0032 +PID_METATRAFFIC_MULTICAST_LOCATOR = 0x0033 +PID_HISTORY = 0x0040 +PID_EXPECTS_INLINE_QOS = 0x0043 +PID_DEFAULT_MULTICAST_LOCATOR = 0x0048 +PID_PARTICIPANT_GUID = 0x0050 +PID_BUILTIN_ENDPOINT_SET = 0x0058 +PID_ENDPOINT_GUID = 0x005A +PID_TYPE_MAX_SIZE_SERIALIZED = 0x0060 +PID_ENTITY_NAME = 0x0062 +PID_KEY_HASH = 0x0070 + +DEFAULT_LEASE_DURATION_SECONDS = 20 +DEFAULT_LEASE_DURATION_NANOSECONDS = 0 +DEFAULT_MAX_BLOCKING_SECONDS = 0 +DEFAULT_MAX_BLOCKING_NANOSECONDS = 100_000_000 +DEFAULT_TOPIC_PREFIX = "espp/rtps_example" +DEFAULT_REQUEST_TOPIC = f"{DEFAULT_TOPIC_PREFIX}/request" +DEFAULT_RESPONSE_TOPIC = f"{DEFAULT_TOPIC_PREFIX}/response" + + +@dataclass +class PortMapping: + metatraffic_multicast: int + metatraffic_unicast: int + user_multicast: int + user_unicast: int + + +@dataclass +class ParticipantProxy: + participant_guid: bytes + guid_prefix: bytes + name: str + enclave: str + address: str + ports: PortMapping + builtin_endpoints: int + + +@dataclass +class EndpointProxy: + guid: bytes + participant_guid: bytes + topic_name: str + type_name: str + reliability: str + is_reader: bool + expects_inline_qos: bool + unicast_address: str + unicast_port: int + + +@dataclass +class WriterConfig: + topic_name: str + type_name: str + reliable: bool + entity_index: int + + +@dataclass +class ReaderConfig: + topic_name: str + type_name: str + reliable: bool + entity_index: int + + +def log(message: str) -> None: + print(message, flush=True) + + +def hex_string(value: bytes) -> str: + return value.hex() + + +def guid_to_string(guid: bytes) -> str: + return hex_string(guid[:12]) + ":" + hex_string(guid[12:]) + + +def entity_id_for_index(entity_index: int, kind: int) -> bytes: + return bytes((0x00, 0x00, 0x10 + entity_index, kind)) + + +def reliability_to_name(reliable: bool) -> str: + return "reliable" if reliable else "best-effort" + + +def compute_port_mapping(domain_id: int, participant_id: int) -> PortMapping: + base = PORT_BASE + DOMAIN_GAIN * domain_id + participant_offset = PARTICIPANT_GAIN * participant_id + return PortMapping( + metatraffic_multicast=base + METATRAFFIC_MULTICAST_OFFSET, + metatraffic_unicast=base + METATRAFFIC_UNICAST_OFFSET + participant_offset, + user_multicast=base + USER_MULTICAST_OFFSET, + user_unicast=base + USER_UNICAST_OFFSET + participant_offset, + ) + + +def guess_local_ipv4() -> str: + probe = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) + try: + probe.connect(("8.8.8.8", 80)) + return probe.getsockname()[0] + except OSError: + return "127.0.0.1" + finally: + probe.close() + + +def make_guid_prefix(node_name: str, domain_id: int, participant_id: int) -> bytes: + digest = hashlib.sha256(node_name.encode("utf-8")).digest() + return bytes( + ( + participant_id & 0xFF, + (participant_id >> 8) & 0xFF, + domain_id & 0xFF, + (domain_id >> 8) & 0xFF, + ) + ) + digest[:8] + + +def make_guid(prefix: bytes, entity_id: bytes) -> bytes: + return prefix + entity_id + + +def align4(buffer: bytearray) -> None: + while len(buffer) % 4 != 0: + buffer.append(0) + + +def append_parameter_header(buffer: bytearray, pid: int, length: int) -> None: + buffer.extend(struct.pack(" None: + append_parameter_header(buffer, pid, 16) + buffer.extend(guid) + + +def append_parameter_protocol_version(buffer: bytearray) -> None: + append_parameter_header(buffer, PID_PROTOCOL_VERSION, 4) + buffer.extend((2, 3, 0, 0)) + + +def append_parameter_vendor_id(buffer: bytearray) -> None: + append_parameter_header(buffer, PID_VENDORID, 4) + buffer.extend(VENDOR_ID) + buffer.extend((0, 0)) + + +def append_parameter_u32(buffer: bytearray, pid: int, value: int) -> None: + append_parameter_header(buffer, pid, 4) + buffer.extend(struct.pack(" None: + append_parameter_header(buffer, pid, 4) + buffer.extend((1 if value else 0, 0, 0, 0)) + + +def append_parameter_duration(buffer: bytearray, pid: int, seconds: int, nanoseconds: int) -> None: + append_parameter_header(buffer, pid, 8) + buffer.extend(struct.pack(" bytes: + locator = bytearray(24) + struct.pack_into(">I", locator, 0, KIND_UDP_V4) + struct.pack_into(">I", locator, 4, port) + locator[20:24] = socket.inet_aton(ip_address) + return bytes(locator) + + +def append_parameter_locator(buffer: bytearray, pid: int, ip_address: str, port: int) -> None: + append_parameter_header(buffer, pid, 24) + buffer.extend(locator_bytes(ip_address, port)) + + +def append_parameter_string_cdr(buffer: bytearray, pid: int, text: str) -> None: + encoded = text.encode("utf-8") + append_parameter_header(buffer, pid, 4 + len(encoded) + 1) + buffer.extend(struct.pack(" None: + append_parameter_header(buffer, pid, 4 + len(payload)) + buffer.extend(struct.pack(" None: + append_parameter_header(buffer, PID_RELIABILITY, 12) + kind = RELIABILITY_RELIABLE if reliable else RELIABILITY_BEST_EFFORT + buffer.extend(struct.pack(" None: + append_parameter_header(buffer, PID_DURABILITY, 4) + buffer.extend(struct.pack(" None: + append_parameter_header(buffer, PID_LIVELINESS, 12) + buffer.extend(struct.pack(" None: + append_parameter_header(buffer, PID_HISTORY, 8) + buffer.extend(struct.pack(" None: + append_parameter_header(buffer, PID_KEY_HASH, 16) + buffer.extend(guid) + + +def append_parameter_sentinel(buffer: bytearray) -> None: + append_parameter_header(buffer, PID_SENTINEL, 0) + + +def build_parameter_list_payload(parameter_buffer: bytearray) -> bytes: + return PL_CDR_LE + bytes(parameter_buffer) + + +def build_data_submessage(reader_id: bytes, writer_id: bytes, sequence_number: int, payload: bytes) -> bytes: + high = sequence_number >> 32 + low = sequence_number & 0xFFFFFFFF + submessage_payload = bytearray() + submessage_payload.extend(struct.pack(" bytes: + header = RTPS_MAGIC + bytes((2, 3)) + VENDOR_ID + guid_prefix + return header + build_data_submessage(reader_id, writer_id, sequence_number, payload) + + +def parse_parameter_list(payload: bytes) -> List[tuple[int, bytes]]: + if len(payload) < 4 or payload[:4] != PL_CDR_LE: + return [] + parameters: List[tuple[int, bytes]] = [] + offset = 4 + while offset + 4 <= len(payload): + pid, length = struct.unpack_from(" len(payload): + return [] + value = payload[offset : offset + length] + parameters.append((pid, value)) + offset += length + offset += (4 - (length % 4)) & 0x3 + return parameters + + +def find_parameter(parameters: Iterable[tuple[int, bytes]], pid: int) -> Optional[bytes]: + for candidate_pid, candidate_value in parameters: + if candidate_pid == pid: + return candidate_value + return None + + +def parse_guid(value: Optional[bytes]) -> Optional[bytes]: + if value is None or len(value) != 16: + return None + return value + + +def parse_u32_le(value: Optional[bytes]) -> Optional[int]: + if value is None or len(value) < 4: + return None + return struct.unpack_from(" Optional[bool]: + if value is None or not value: + return None + return value[0] != 0 + + +def parse_cdr_string(value: Optional[bytes]) -> Optional[str]: + if value is None or len(value) < 4: + return None + length = struct.unpack_from(" len(value): + return None + raw = value[4 : 4 + length] + if raw.endswith(b"\x00"): + raw = raw[:-1] + return raw.decode("utf-8", errors="replace") + + +def parse_octet_sequence(value: Optional[bytes]) -> Optional[bytes]: + if value is None or len(value) < 4: + return None + length = struct.unpack_from(" len(value): + return None + return value[4 : 4 + length] + + +def parse_locator(value: Optional[bytes]) -> tuple[str, int]: + if value is None or len(value) != 24: + return ("0.0.0.0", 0) + kind = struct.unpack_from(">I", value, 0)[0] + if kind != KIND_UDP_V4: + return ("0.0.0.0", 0) + port = struct.unpack_from(">I", value, 4)[0] + ip_address = socket.inet_ntoa(value[20:24]) + return (ip_address, port) + + +def parse_reliability(value: Optional[bytes]) -> str: + kind = parse_u32_le(value) + return "reliable" if kind == RELIABILITY_RELIABLE else "best-effort" + + +def extract_enclave(value: Optional[bytes]) -> str: + if not value: + return "/" + text = value.decode("utf-8", errors="replace") + marker = "enclave=" + start = text.find(marker) + if start < 0: + return "/" + start += len(marker) + end = text.find(";", start) + if end < 0: + end = len(text) + return text[start:end] or "/" + + +def serialize_uint32_cdr(value: int) -> bytes: + return b"\x00\x01\x00\x00" + struct.pack(" Optional[int]: + if len(payload) < 8 or payload[:2] != b"\x00\x01": + return None + return struct.unpack_from(" None: + self.args = args + self.ports = compute_port_mapping(args.domain_id, args.participant_id) + self.guid_prefix = make_guid_prefix(args.node_name, args.domain_id, args.participant_id) + self.participant_guid = make_guid(self.guid_prefix, PARTICIPANT_ENTITY_ID) + self.sequence_numbers: Dict[bytes, int] = {} + self.discovered_participants: Dict[bytes, ParticipantProxy] = {} + self.discovered_writers: Dict[bytes, EndpointProxy] = {} + self.discovered_readers: Dict[bytes, EndpointProxy] = {} + + self.local_writers = [ + WriterConfig( + topic_name=args.publish_topic, + type_name=args.type_name, + reliable=args.reliable, + entity_index=0, + ) + ] if args.publish_topic else [] + self.local_readers = [ + ReaderConfig( + topic_name=topic_name, + type_name=args.type_name, + reliable=False, + entity_index=index, + ) + for index, topic_name in enumerate(args.subscribe_topic) + ] + + self.metatraffic_multicast_sock = self._create_metatraffic_multicast_socket() + self.metatraffic_unicast_sock = self._create_bound_udp_socket(self.ports.metatraffic_unicast) + self.user_unicast_sock = self._create_bound_udp_socket(self.ports.user_unicast) + self._configure_multicast_sender(self.metatraffic_unicast_sock) + + self.next_discovery_send = 0.0 + self.next_publish_send = 0.0 + self.last_no_participant_log = 0.0 + + def _create_bound_udp_socket(self, port: int) -> socket.socket: + sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM, socket.IPPROTO_UDP) + sock.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1) + if hasattr(socket, "SO_REUSEPORT"): + try: + sock.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEPORT, 1) + except OSError: + pass + sock.bind((self.args.bind_address, port)) + sock.setblocking(False) + return sock + + def _create_metatraffic_multicast_socket(self) -> socket.socket: + sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM, socket.IPPROTO_UDP) + sock.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1) + if hasattr(socket, "SO_REUSEPORT"): + try: + sock.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEPORT, 1) + except OSError: + pass + sock.bind(("", self.ports.metatraffic_multicast)) + interface_ip = self.args.multicast_interface or self.args.advertised_address + membership = socket.inet_aton(self.args.multicast_group) + socket.inet_aton(interface_ip) + sock.setsockopt(socket.IPPROTO_IP, socket.IP_ADD_MEMBERSHIP, membership) + sock.setblocking(False) + return sock + + def _configure_multicast_sender(self, sock: socket.socket) -> None: + sock.setsockopt(socket.IPPROTO_IP, socket.IP_MULTICAST_TTL, 1) + sock.setsockopt(socket.IPPROTO_IP, socket.IP_MULTICAST_LOOP, 1) + interface_ip = self.args.multicast_interface or self.args.advertised_address + sock.setsockopt(socket.IPPROTO_IP, socket.IP_MULTICAST_IF, socket.inet_aton(interface_ip)) + + def _next_sequence(self, writer_entity_id: bytes) -> int: + value = self.sequence_numbers.get(writer_entity_id, 1) + self.sequence_numbers[writer_entity_id] = value + 1 + return value + + def _local_writer_guid(self, entity_index: int) -> bytes: + return make_guid(self.guid_prefix, entity_id_for_index(entity_index, USER_WRITER_NO_KEY_KIND)) + + def _local_reader_guid(self, entity_index: int) -> bytes: + return make_guid(self.guid_prefix, entity_id_for_index(entity_index, USER_READER_NO_KEY_KIND)) + + def build_spdp_announce_message(self) -> bytes: + parameters = bytearray() + append_parameter_protocol_version(parameters) + append_parameter_vendor_id(parameters) + append_parameter_u32(parameters, PID_DOMAIN_ID, self.args.domain_id) + append_parameter_guid(parameters, PID_PARTICIPANT_GUID, self.participant_guid) + append_parameter_locator( + parameters, + PID_METATRAFFIC_MULTICAST_LOCATOR, + self.args.multicast_group, + self.ports.metatraffic_multicast, + ) + append_parameter_locator( + parameters, + PID_METATRAFFIC_UNICAST_LOCATOR, + self.args.advertised_address, + self.ports.metatraffic_unicast, + ) + append_parameter_locator( + parameters, + PID_DEFAULT_UNICAST_LOCATOR, + self.args.advertised_address, + self.ports.user_unicast, + ) + append_parameter_locator( + parameters, + PID_DEFAULT_MULTICAST_LOCATOR, + self.args.multicast_group, + self.ports.user_multicast, + ) + append_parameter_duration( + parameters, + PID_PARTICIPANT_LEASE_DURATION, + DEFAULT_LEASE_DURATION_SECONDS, + DEFAULT_LEASE_DURATION_NANOSECONDS, + ) + append_parameter_u32(parameters, PID_BUILTIN_ENDPOINT_SET, BUILTIN_ENDPOINT_SET) + append_parameter_octet_sequence( + parameters, + PID_USER_DATA, + f"enclave={self.args.enclave};".encode("utf-8"), + ) + append_parameter_string_cdr(parameters, PID_ENTITY_NAME, self.args.node_name) + append_parameter_sentinel(parameters) + payload = build_parameter_list_payload(parameters) + return build_rtps_message( + self.guid_prefix, + ENTITY_ID_UNKNOWN, + SPDP_WRITER_ENTITY_ID, + self._next_sequence(SPDP_WRITER_ENTITY_ID), + payload, + ) + + def build_sedp_publication_message(self, writer: WriterConfig) -> bytes: + guid = self._local_writer_guid(writer.entity_index) + parameters = bytearray() + append_parameter_guid(parameters, PID_ENDPOINT_GUID, guid) + append_parameter_locator(parameters, PID_UNICAST_LOCATOR, self.args.advertised_address, self.ports.user_unicast) + append_parameter_guid(parameters, PID_PARTICIPANT_GUID, self.participant_guid) + append_parameter_string_cdr(parameters, PID_TOPIC_NAME, writer.topic_name) + append_parameter_string_cdr(parameters, PID_TYPE_NAME, writer.type_name) + append_parameter_key_hash(parameters, guid) + append_parameter_u32(parameters, PID_TYPE_MAX_SIZE_SERIALIZED, 8) + append_parameter_protocol_version(parameters) + append_parameter_vendor_id(parameters) + append_parameter_durability(parameters) + append_parameter_liveliness(parameters) + append_parameter_reliability(parameters, writer.reliable) + append_parameter_history(parameters) + append_parameter_sentinel(parameters) + payload = build_parameter_list_payload(parameters) + return build_rtps_message( + self.guid_prefix, + SEDP_PUBLICATIONS_READER_ENTITY_ID, + SEDP_PUBLICATIONS_WRITER_ENTITY_ID, + self._next_sequence(SEDP_PUBLICATIONS_WRITER_ENTITY_ID), + payload, + ) + + def build_sedp_subscription_message(self, reader: ReaderConfig) -> bytes: + guid = self._local_reader_guid(reader.entity_index) + parameters = bytearray() + append_parameter_guid(parameters, PID_ENDPOINT_GUID, guid) + append_parameter_locator(parameters, PID_UNICAST_LOCATOR, self.args.advertised_address, self.ports.user_unicast) + append_parameter_bool(parameters, PID_EXPECTS_INLINE_QOS, False) + append_parameter_guid(parameters, PID_PARTICIPANT_GUID, self.participant_guid) + append_parameter_string_cdr(parameters, PID_TOPIC_NAME, reader.topic_name) + append_parameter_string_cdr(parameters, PID_TYPE_NAME, reader.type_name) + append_parameter_key_hash(parameters, guid) + append_parameter_protocol_version(parameters) + append_parameter_vendor_id(parameters) + append_parameter_durability(parameters) + append_parameter_liveliness(parameters) + append_parameter_reliability(parameters, reader.reliable) + append_parameter_history(parameters) + append_parameter_sentinel(parameters) + payload = build_parameter_list_payload(parameters) + return build_rtps_message( + self.guid_prefix, + SEDP_SUBSCRIPTIONS_READER_ENTITY_ID, + SEDP_SUBSCRIPTIONS_WRITER_ENTITY_ID, + self._next_sequence(SEDP_SUBSCRIPTIONS_WRITER_ENTITY_ID), + payload, + ) + + def build_uint32_data_message(self, writer: WriterConfig, value: int) -> bytes: + payload = bytearray() + payload.extend(USER_DATA_MAGIC) + payload.append(USER_DATA_VERSION) + payload.append(RELIABILITY_RELIABLE if writer.reliable else RELIABILITY_BEST_EFFORT) + topic_name = writer.topic_name.encode("utf-8") + payload.extend(struct.pack(" None: + payload = self.build_spdp_announce_message() + self.metatraffic_unicast_sock.sendto( + payload, + (self.args.multicast_group, self.ports.metatraffic_multicast), + ) + + def send_sedp_announcements_to(self, participant: ParticipantProxy) -> None: + target = (participant.address, participant.ports.metatraffic_unicast) + if participant.ports.metatraffic_unicast == 0 or not participant.address: + return + for writer in self.local_writers: + self.metatraffic_unicast_sock.sendto(self.build_sedp_publication_message(writer), target) + for reader in self.local_readers: + self.metatraffic_unicast_sock.sendto(self.build_sedp_subscription_message(reader), target) + + def send_discovery_now(self) -> None: + self.send_spdp_announce_now() + for participant in list(self.discovered_participants.values()): + self.send_sedp_announcements_to(participant) + + def publish_now(self) -> None: + if not self.local_writers: + return + if not self._publish_value(self.local_writers[0], self.args.publish_value): + now = time.monotonic() + if now - self.last_no_participant_log > 2.0: + log( + f"[publish] no discovered participants yet for topic '{self.local_writers[0].topic_name}', " + "waiting for SPDP" + ) + self.last_no_participant_log = now + else: + log( + f"[publish] sent {self.args.publish_value} on '{self.local_writers[0].topic_name}' " + f"to {len(self.discovered_participants)} discovered participant(s)" + ) + + def _publish_value(self, writer: WriterConfig, value: int, target: Optional[tuple[str, int]] = None) -> bool: + payload = self.build_uint32_data_message(writer, value) + if target is not None: + self.user_unicast_sock.sendto(payload, target) + return True + if not self.discovered_participants: + return False + for participant in self.discovered_participants.values(): + self.user_unicast_sock.sendto(payload, (participant.address, participant.ports.user_unicast)) + return True + + def handle_metatraffic_packet(self, packet: bytes, sender_ip: str) -> None: + for writer_id, serialized_payload in parse_rtps_data_messages(packet): + parameters = parse_parameter_list(serialized_payload) + if not parameters: + continue + + if writer_id == SPDP_WRITER_ENTITY_ID: + self._handle_spdp(parameters, sender_ip) + elif writer_id == SEDP_PUBLICATIONS_WRITER_ENTITY_ID: + self._handle_sedp(parameters, sender_ip, is_reader=False) + elif writer_id == SEDP_SUBSCRIPTIONS_WRITER_ENTITY_ID: + self._handle_sedp(parameters, sender_ip, is_reader=True) + + def _handle_spdp(self, parameters: List[tuple[int, bytes]], sender_ip: str) -> None: + participant_guid = parse_guid(find_parameter(parameters, PID_PARTICIPANT_GUID)) + if participant_guid is None or participant_guid[:12] == self.guid_prefix: + return + + meta_ip, meta_uc_port = parse_locator(find_parameter(parameters, PID_METATRAFFIC_UNICAST_LOCATOR)) + _, meta_mc_port = parse_locator(find_parameter(parameters, PID_METATRAFFIC_MULTICAST_LOCATOR)) + user_ip, user_uc_port = parse_locator(find_parameter(parameters, PID_DEFAULT_UNICAST_LOCATOR)) + _, user_mc_port = parse_locator(find_parameter(parameters, PID_DEFAULT_MULTICAST_LOCATOR)) + + participant = ParticipantProxy( + participant_guid=participant_guid, + guid_prefix=participant_guid[:12], + name=parse_cdr_string(find_parameter(parameters, PID_ENTITY_NAME)) or "", + enclave=extract_enclave(parse_octet_sequence(find_parameter(parameters, PID_USER_DATA))), + address=user_ip if user_ip != "0.0.0.0" else sender_ip, + ports=PortMapping( + metatraffic_multicast=meta_mc_port, + metatraffic_unicast=meta_uc_port, + user_multicast=user_mc_port, + user_unicast=user_uc_port, + ), + builtin_endpoints=parse_u32_le(find_parameter(parameters, PID_BUILTIN_ENDPOINT_SET)) or 0, + ) + + is_new = participant_guid not in self.discovered_participants + self.discovered_participants[participant_guid] = participant + label = participant.name or hex_string(participant.guid_prefix) + log( + f"[spdp] participant '{label}' at {participant.address} " + f"(meta={participant.ports.metatraffic_unicast}, user={participant.ports.user_unicast}, " + f"enclave={participant.enclave})" + ) + if is_new: + self.send_sedp_announcements_to(participant) + + def _handle_sedp(self, parameters: List[tuple[int, bytes]], sender_ip: str, is_reader: bool) -> None: + endpoint_guid = parse_guid(find_parameter(parameters, PID_ENDPOINT_GUID)) + if endpoint_guid is None or endpoint_guid[:12] == self.guid_prefix: + return + + participant_guid = parse_guid(find_parameter(parameters, PID_PARTICIPANT_GUID)) + if participant_guid is None: + participant_guid = endpoint_guid[:12] + PARTICIPANT_ENTITY_ID + + endpoint_ip, endpoint_port = parse_locator(find_parameter(parameters, PID_UNICAST_LOCATOR)) + endpoint = EndpointProxy( + guid=endpoint_guid, + participant_guid=participant_guid, + topic_name=parse_cdr_string(find_parameter(parameters, PID_TOPIC_NAME)) or "", + type_name=parse_cdr_string(find_parameter(parameters, PID_TYPE_NAME)) or "", + reliability=parse_reliability(find_parameter(parameters, PID_RELIABILITY)), + is_reader=is_reader, + expects_inline_qos=parse_bool(find_parameter(parameters, PID_EXPECTS_INLINE_QOS)) or False, + unicast_address=endpoint_ip if endpoint_ip != "0.0.0.0" else sender_ip, + unicast_port=endpoint_port, + ) + endpoint_map = self.discovered_readers if is_reader else self.discovered_writers + is_new = endpoint_guid not in endpoint_map + endpoint_map[endpoint_guid] = endpoint + if is_new: + kind = "reader" if is_reader else "writer" + log( + f"[sedp] {kind} topic='{endpoint.topic_name}' type='{endpoint.type_name}' " + f"reliability={endpoint.reliability} participant={guid_to_string(endpoint.participant_guid)}" + ) + + def handle_user_packet(self, packet: bytes, sender_ip: str, sender_port: int) -> None: + for writer_id, serialized_payload in parse_rtps_data_messages(packet): + if not serialized_payload.startswith(USER_DATA_MAGIC) or len(serialized_payload) < len(USER_DATA_MAGIC) + 2: + continue + offset = len(USER_DATA_MAGIC) + version = serialized_payload[offset] + reliability = serialized_payload[offset + 1] + offset += 2 + if version != USER_DATA_VERSION: + continue + if offset + 2 > len(serialized_payload): + continue + topic_length = struct.unpack_from(" len(serialized_payload): + continue + topic_name = serialized_payload[offset : offset + topic_length].decode("utf-8", errors="replace") + offset += topic_length + if offset + 2 > len(serialized_payload): + continue + payload_length = struct.unpack_from(" len(serialized_payload): + continue + maybe_value = deserialize_uint32_cdr(serialized_payload[offset : offset + payload_length]) + if maybe_value is None: + continue + reliability_name = "reliable" if reliability == RELIABILITY_RELIABLE else "best-effort" + log( + f"[data] topic='{topic_name}' value={maybe_value} reliability={reliability_name} " + f"from {sender_ip}:{sender_port} writer={hex_string(writer_id)}" + ) + if self.args.echo_received and self.local_writers: + subscribed_topics = {reader.topic_name for reader in self.local_readers} + if topic_name in subscribed_topics: + writer = self.local_writers[0] + self._publish_value(writer, maybe_value, (sender_ip, sender_port)) + log( + f"[echo] responded with value={maybe_value} on '{writer.topic_name}' " + f"to {sender_ip}:{sender_port}" + ) + + def run(self) -> None: + start_time = time.monotonic() + self.next_discovery_send = start_time + self.next_publish_send = start_time + self.args.publish_interval + + log( + "Starting RTPS host harness\n" + f" node: {self.args.node_name}\n" + f" advertised address: {self.args.advertised_address}\n" + f" domain/participant: {self.args.domain_id}/{self.args.participant_id}\n" + f" ports: meta_mc={self.ports.metatraffic_multicast}, meta_uc={self.ports.metatraffic_unicast}, " + f"user_mc={self.ports.user_multicast}, user_uc={self.ports.user_unicast}" + ) + if self.local_readers: + log(" readers: " + ", ".join(reader.topic_name for reader in self.local_readers)) + if self.local_writers: + writer = self.local_writers[0] + writer_mode = "echo responder" if self.args.echo_received else "periodic publisher" + interval_text = ( + f", publish value={self.args.publish_value} every {self.args.publish_interval:.2f}s" + if self.args.publish_interval > 0 + else "" + ) + log(f" writer: {writer.topic_name} ({reliability_to_name(writer.reliable)}, {writer_mode}{interval_text})") + + try: + while True: + now = time.monotonic() + if now >= self.next_discovery_send: + self.send_discovery_now() + self.next_discovery_send = now + self.args.announce_period + if self.local_writers and self.args.publish_interval > 0 and now >= self.next_publish_send: + self.publish_now() + self.next_publish_send = now + self.args.publish_interval + if self.args.duration > 0 and now - start_time >= self.args.duration: + break + + readable, _, _ = select.select( + [ + self.metatraffic_multicast_sock, + self.metatraffic_unicast_sock, + self.user_unicast_sock, + ], + [], + [], + 0.2, + ) + for sock in readable: + packet, sender = sock.recvfrom(4096) + sender_ip, sender_port = sender[0], sender[1] + if sock is self.user_unicast_sock: + self.handle_user_packet(packet, sender_ip, sender_port) + else: + self.handle_metatraffic_packet(packet, sender_ip) + except KeyboardInterrupt: + log("Stopping RTPS host harness") + finally: + self.close() + + def close(self) -> None: + for sock in ( + self.metatraffic_multicast_sock, + self.metatraffic_unicast_sock, + self.user_unicast_sock, + ): + try: + sock.close() + except OSError: + pass + + +def parse_rtps_data_messages(packet: bytes) -> List[tuple[bytes, bytes]]: + if len(packet) < 20 or not packet.startswith(RTPS_MAGIC): + return [] + offset = 20 + messages: List[tuple[bytes, bytes]] = [] + while offset + 4 <= len(packet): + kind = packet[offset] + flags = packet[offset + 1] + length = struct.unpack_from(" len(packet): + break + payload = packet[offset : offset + length] + offset += length + if kind != DATA_SUBMESSAGE_KIND or (flags & 0x04) == 0: + continue + if len(payload) < 20: + continue + extra_flags, octets_to_inline_qos = struct.unpack_from(" argparse.Namespace: + parser = argparse.ArgumentParser( + description=( + "Discover an ESPP RTPS participant from a PC/host and optionally " + "exchange temporary UInt32 user-data samples." + ) + ) + parser.add_argument("--node-name", default="python_rtps_host", help="Local participant name") + parser.add_argument("--domain-id", type=int, default=0, help="RTPS domain id") + parser.add_argument("--participant-id", type=int, default=10, help="Local participant id") + parser.add_argument("--bind-address", default="0.0.0.0", help="Local bind address") + parser.add_argument( + "--advertised-address", + default=None, + help="IPv4 address to advertise to peers (defaults to best-effort local IPv4)", + ) + parser.add_argument( + "--multicast-interface", + default=None, + help="IPv4 interface to use for multicast join/send (defaults to advertised address)", + ) + parser.add_argument("--multicast-group", default="239.255.0.1", help="RTPS metatraffic multicast group") + parser.add_argument("--enclave", default="/", help="Enclave string advertised in SPDP user data") + parser.add_argument( + "--subscribe-topic", + action="append", + default=None, + help=f"Topic name to advertise as a local reader (repeatable). Defaults to {DEFAULT_REQUEST_TOPIC}.", + ) + parser.add_argument( + "--publish-topic", + default=None, + help=f"Topic name to publish as a local writer. Defaults to {DEFAULT_RESPONSE_TOPIC}.", + ) + parser.add_argument("--publish-value", type=int, default=42, help="UInt32 value to publish") + parser.add_argument( + "--publish-interval", + type=float, + default=0.0, + help="Seconds between periodic publish attempts when --publish-topic is set (0 disables periodic publishing)", + ) + parser.set_defaults(echo_received=True) + parser.add_argument( + "--echo-received", + dest="echo_received", + action="store_true", + help="Echo received subscribed-topic values back on the publish topic (enabled by default)", + ) + parser.add_argument( + "--no-echo-received", + dest="echo_received", + action="store_false", + help="Disable request/response echo behavior and only use periodic publishing", + ) + parser.add_argument("--reliable", action="store_true", help="Mark the local writer as reliable") + parser.add_argument("--type-name", default="std_msgs/msg/UInt32", help="Advertised type name") + parser.add_argument( + "--announce-period", + type=float, + default=1.0, + help="Seconds between periodic SPDP/SEDP discovery announcements", + ) + parser.add_argument( + "--duration", + type=float, + default=0.0, + help="Stop after this many seconds (0 = run until Ctrl+C)", + ) + args = parser.parse_args() + + if args.subscribe_topic is None: + args.subscribe_topic = [DEFAULT_REQUEST_TOPIC] + if args.publish_topic is None: + args.publish_topic = DEFAULT_RESPONSE_TOPIC + if args.advertised_address is None: + args.advertised_address = guess_local_ipv4() + try: + ipaddress.IPv4Address(args.advertised_address) + ipaddress.IPv4Address(args.multicast_interface or args.advertised_address) + ipaddress.IPv4Address(args.multicast_group) + except ipaddress.AddressValueError as exc: + parser.error(str(exc)) + if args.publish_interval < 0: + parser.error("--publish-interval must be >= 0") + if args.announce_period <= 0: + parser.error("--announce-period must be > 0") + return args + + +def main() -> int: + args = parse_args() + harness = RtpsHostHarness(args) + harness.run() + return 0 + + +if __name__ == "__main__": + sys.exit(main()) From 293a9fd4e58b77ae92842c6a2f606ee69575085e Mon Sep 17 00:00:00 2001 From: William Emfinger Date: Mon, 15 Jun 2026 14:24:22 -0500 Subject: [PATCH 02/32] address comments --- components/cdr/include/cdr.hpp | 17 ++++++---- components/rtps/example/main/rtps_example.cpp | 3 +- components/rtps/src/rtps.cpp | 15 +++++---- doc/conf_common.py | 2 +- python/rtps_host.py | 32 +++++++++++++++---- 5 files changed, 47 insertions(+), 22 deletions(-) diff --git a/components/cdr/include/cdr.hpp b/components/cdr/include/cdr.hpp index a3c8a5944..b2c01bf29 100644 --- a/components/cdr/include/cdr.hpp +++ b/components/cdr/include/cdr.hpp @@ -207,12 +207,8 @@ class CdrWriter { template requires(std::is_integral_v || std::is_floating_point_v) bool write_array(const std::array &values) { - for (const auto &value : values) { - if (!write(value)) { - return false; - } - } - return true; + return std::all_of(values.begin(), values.end(), + [this](const auto &value) { return write(value); }); } /// @brief Append a variable-length CDR sequence of primitive values. @@ -376,7 +372,14 @@ class CdrReader { /// @return True if a complete value was decoded, false otherwise. template requires(std::is_integral_v || std::is_floating_point_v) bool read(T &value) { - if (!align(detail::cdr_alignment()) || remaining() < sizeof(T)) { + constexpr size_t alignment = detail::cdr_alignment(); + if constexpr (alignment > 1) { + if (!align(alignment)) { + valid_ = false; + return false; + } + } + if (remaining() < sizeof(T)) { valid_ = false; return false; } diff --git a/components/rtps/example/main/rtps_example.cpp b/components/rtps/example/main/rtps_example.cpp index 2abce874f..4786347f7 100644 --- a/components/rtps/example/main/rtps_example.cpp +++ b/components/rtps/example/main/rtps_example.cpp @@ -96,7 +96,6 @@ extern "C" void app_main(void) { std::atomic response_count{0}; std::atomic next_request_value{1}; std::atomic last_sent_request{0}; - espp::RtpsParticipant *participant_ptr = nullptr; espp::RtpsParticipant participant({ .node_name = node_name, @@ -117,7 +116,6 @@ extern "C" void app_main(void) { }, .log_level = espp::Logger::Verbosity::INFO, }); - participant_ptr = &participant; #if CONFIG_RTPS_EXAMPLE_ROLE_INITIATOR participant.add_writer({ @@ -138,6 +136,7 @@ extern "C" void app_main(void) { }, }); #else + auto *participant_ptr = &participant; participant.add_writer({ .topic_name = response_topic, .type_name = std::string(kTypeName), diff --git a/components/rtps/src/rtps.cpp b/components/rtps/src/rtps.cpp index df526118c..04ca02097 100644 --- a/components/rtps/src/rtps.cpp +++ b/components/rtps/src/rtps.cpp @@ -4,6 +4,7 @@ #include #include #include +#include #include #include @@ -1039,7 +1040,10 @@ bool RtpsParticipant::publish_uint32(std::string_view topic_name, uint32_t value logger_.warn("Reliable user-data retransmission is not implemented yet; sending best-effort"); } - auto payload = build_uint32_data_message(topic_name, value, writer_config.reliability); + auto encoded_reliability = writer_config.reliability == ReliabilityKind::RELIABLE + ? ReliabilityKind::BEST_EFFORT + : writer_config.reliability; + auto payload = build_uint32_data_message(topic_name, value, encoded_reliability); auto participants = discovered_participants(); if (participants.empty()) { logger_.warn("No discovered participants available for topic '{}'", topic_name); @@ -1393,12 +1397,11 @@ bool RtpsParticipant::send_sedp_announcements_to(const ParticipantProxy &partici } bool RtpsParticipant::send_discovery_now() { - auto sent = send_spdp_announce_now(); auto participants = discovered_participants(); - for (const auto &participant : participants) { - sent = send_sedp_announcements_to(participant) || sent; - } - return sent; + return std::accumulate(participants.begin(), participants.end(), send_spdp_announce_now(), + [this](bool sent, const auto &participant) { + return send_sedp_announcements_to(participant) || sent; + }); } RtpsParticipant::ParticipantProxy RtpsParticipant::make_local_participant_proxy() const { diff --git a/doc/conf_common.py b/doc/conf_common.py index 6cc7137d2..526875180 100644 --- a/doc/conf_common.py +++ b/doc/conf_common.py @@ -13,7 +13,7 @@ mermaid_dark_theme = 'neutral' mermaid_light_theme = 'neutral' -exclude_paterns = ['build', '_build', 'detail'] +exclude_patterns = ['build', '_build', 'detail'] # link roles config github_repo = 'esp-cpp/espp' diff --git a/python/rtps_host.py b/python/rtps_host.py index 81eef251b..ece9fc617 100644 --- a/python/rtps_host.py +++ b/python/rtps_host.py @@ -47,6 +47,8 @@ RELIABILITY_BEST_EFFORT = 1 RELIABILITY_RELIABLE = 2 +USER_DATA_RELIABILITY_BEST_EFFORT = 0 +USER_DATA_RELIABILITY_RELIABLE = 1 KIND_UDP_V4 = 1 VENDOR_ID = b"\xca\xfe" @@ -490,6 +492,8 @@ def _create_bound_udp_socket(self, port: int) -> socket.socket: try: sock.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEPORT, 1) except OSError: + # Some platforms expose SO_REUSEPORT but reject setting it; this is + # a best-effort optimization and is not required for correctness. pass sock.bind((self.args.bind_address, port)) sock.setblocking(False) @@ -502,8 +506,15 @@ def _create_metatraffic_multicast_socket(self) -> socket.socket: try: sock.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEPORT, 1) except OSError: + # Some platforms expose SO_REUSEPORT but reject setting it; this is + # a best-effort optimization and is not required for correctness. pass - sock.bind(("", self.ports.metatraffic_multicast)) + try: + sock.bind((self.args.multicast_group, self.ports.metatraffic_multicast)) + except OSError: + # Not all platforms allow binding directly to the multicast group + # address, so fall back to the selected local interface address. + sock.bind((self.args.bind_address, self.ports.metatraffic_multicast)) interface_ip = self.args.multicast_interface or self.args.advertised_address membership = socket.inet_aton(self.args.multicast_group) + socket.inet_aton(interface_ip) sock.setsockopt(socket.IPPROTO_IP, socket.IP_ADD_MEMBERSHIP, membership) @@ -636,7 +647,7 @@ def build_uint32_data_message(self, writer: WriterConfig, value: int) -> bytes: payload = bytearray() payload.extend(USER_DATA_MAGIC) payload.append(USER_DATA_VERSION) - payload.append(RELIABILITY_RELIABLE if writer.reliable else RELIABILITY_BEST_EFFORT) + payload.append(USER_DATA_RELIABILITY_RELIABLE if writer.reliable else USER_DATA_RELIABILITY_BEST_EFFORT) topic_name = writer.topic_name.encode("utf-8") payload.extend(struct.pack(" maybe_value = deserialize_uint32_cdr(serialized_payload[offset : offset + payload_length]) if maybe_value is None: continue - reliability_name = "reliable" if reliability == RELIABILITY_RELIABLE else "best-effort" + reliability_name = ( + "reliable" if reliability == USER_DATA_RELIABILITY_RELIABLE else "best-effort" + ) log( f"[data] topic='{topic_name}' value={maybe_value} reliability={reliability_name} " f"from {sender_ip}:{sender_port} writer={hex_string(writer_id)}" @@ -890,8 +903,8 @@ def close(self) -> None: ): try: sock.close() - except OSError: - pass + except OSError as exc: + log(f"[close] ignoring socket close failure for {sock!r}: {exc}") def parse_rtps_data_messages(packet: bytes) -> List[tuple[bytes, bytes]]: @@ -932,7 +945,11 @@ def parse_args() -> argparse.Namespace: parser.add_argument("--node-name", default="python_rtps_host", help="Local participant name") parser.add_argument("--domain-id", type=int, default=0, help="RTPS domain id") parser.add_argument("--participant-id", type=int, default=10, help="Local participant id") - parser.add_argument("--bind-address", default="0.0.0.0", help="Local bind address") + parser.add_argument( + "--bind-address", + default=None, + help="Local bind address (defaults to the advertised address rather than all interfaces)", + ) parser.add_argument( "--advertised-address", default=None, @@ -998,7 +1015,10 @@ def parse_args() -> argparse.Namespace: args.publish_topic = DEFAULT_RESPONSE_TOPIC if args.advertised_address is None: args.advertised_address = guess_local_ipv4() + if args.bind_address is None: + args.bind_address = args.advertised_address try: + ipaddress.IPv4Address(args.bind_address) ipaddress.IPv4Address(args.advertised_address) ipaddress.IPv4Address(args.multicast_interface or args.advertised_address) ipaddress.IPv4Address(args.multicast_group) From f9f4317ac613a843a307495ce90eb6462b6b77c8 Mon Sep 17 00:00:00 2001 From: William Emfinger Date: Mon, 15 Jun 2026 15:05:10 -0500 Subject: [PATCH 03/32] improve code --- components/cdr/include/cdr.hpp | 7 +++++-- python/rtps_host.py | 22 ++++++++++++++-------- 2 files changed, 19 insertions(+), 10 deletions(-) diff --git a/components/cdr/include/cdr.hpp b/components/cdr/include/cdr.hpp index b2c01bf29..42f3ec1f2 100644 --- a/components/cdr/include/cdr.hpp +++ b/components/cdr/include/cdr.hpp @@ -149,13 +149,16 @@ class CdrWriter { /// @brief Pad the buffer with zeros until it satisfies the requested alignment. /// @param alignment Required alignment in bytes. Values less than or equal to 1 are ignored. - void align(size_t alignment) { + /// @return Always returns true. This matches the reader API even though writer-side alignment + /// cannot currently fail. + bool align(size_t alignment) { if (alignment <= 1) { - return; + return true; } while (data_.size() % alignment != 0) { data_.push_back(0); } + return true; } /// @brief Append a primitive scalar using CDR alignment and endianness rules. diff --git a/python/rtps_host.py b/python/rtps_host.py index ece9fc617..8c30e92ec 100644 --- a/python/rtps_host.py +++ b/python/rtps_host.py @@ -45,8 +45,8 @@ DATA_SUBMESSAGE_FLAGS = 0x01 | 0x04 DATA_SUBMESSAGE_OCTETS_TO_INLINE_QOS = 16 -RELIABILITY_BEST_EFFORT = 1 -RELIABILITY_RELIABLE = 2 +RTPS_QOS_RELIABILITY_BEST_EFFORT = 1 +RTPS_QOS_RELIABILITY_RELIABLE = 2 USER_DATA_RELIABILITY_BEST_EFFORT = 0 USER_DATA_RELIABILITY_RELIABLE = 1 @@ -177,6 +177,14 @@ def reliability_to_name(reliable: bool) -> str: return "reliable" if reliable else "best-effort" +def encode_user_data_reliability(reliable: bool) -> int: + return USER_DATA_RELIABILITY_RELIABLE if reliable else USER_DATA_RELIABILITY_BEST_EFFORT + + +def decode_user_data_reliability(encoded: int) -> str: + return "reliable" if encoded == USER_DATA_RELIABILITY_RELIABLE else "best-effort" + + def compute_port_mapping(domain_id: int, participant_id: int) -> PortMapping: base = PORT_BASE + DOMAIN_GAIN * domain_id participant_offset = PARTICIPANT_GAIN * participant_id @@ -286,7 +294,7 @@ def append_parameter_octet_sequence(buffer: bytearray, pid: int, payload: bytes) def append_parameter_reliability(buffer: bytearray, reliable: bool) -> None: append_parameter_header(buffer, PID_RELIABILITY, 12) - kind = RELIABILITY_RELIABLE if reliable else RELIABILITY_BEST_EFFORT + kind = RTPS_QOS_RELIABILITY_RELIABLE if reliable else RTPS_QOS_RELIABILITY_BEST_EFFORT buffer.extend(struct.pack(" tuple[str, int]: def parse_reliability(value: Optional[bytes]) -> str: kind = parse_u32_le(value) - return "reliable" if kind == RELIABILITY_RELIABLE else "best-effort" + return "reliable" if kind == RTPS_QOS_RELIABILITY_RELIABLE else "best-effort" def extract_enclave(value: Optional[bytes]) -> str: @@ -647,7 +655,7 @@ def build_uint32_data_message(self, writer: WriterConfig, value: int) -> bytes: payload = bytearray() payload.extend(USER_DATA_MAGIC) payload.append(USER_DATA_VERSION) - payload.append(USER_DATA_RELIABILITY_RELIABLE if writer.reliable else USER_DATA_RELIABILITY_BEST_EFFORT) + payload.append(encode_user_data_reliability(writer.reliable)) topic_name = writer.topic_name.encode("utf-8") payload.extend(struct.pack(" maybe_value = deserialize_uint32_cdr(serialized_payload[offset : offset + payload_length]) if maybe_value is None: continue - reliability_name = ( - "reliable" if reliability == USER_DATA_RELIABILITY_RELIABLE else "best-effort" - ) + reliability_name = decode_user_data_reliability(reliability) log( f"[data] topic='{topic_name}' value={maybe_value} reliability={reliability_name} " f"from {sender_ip}:{sender_port} writer={hex_string(writer_id)}" From 64634d9776ed9f396f84658bba1e1c8cf110e895 Mon Sep 17 00:00:00 2001 From: William Emfinger Date: Mon, 15 Jun 2026 15:11:11 -0500 Subject: [PATCH 04/32] update readmes --- components/rtps/README.md | 54 +++++++++++++++++++++++++++++++++ components/rtsp/README.md | 64 +++++++++++++++++++++++++++++++++++++++ 2 files changed, 118 insertions(+) diff --git a/components/rtps/README.md b/components/rtps/README.md index 05ec19090..c9babda33 100644 --- a/components/rtps/README.md +++ b/components/rtps/README.md @@ -22,6 +22,60 @@ standards-shaped, but the reliable RTPS state machines (`HEARTBEAT`, `ACKNACK`, resend windows) and ROS 2 endpoint/user-data interoperability are still incomplete. +## How RTPS Works + +RTPS separates *metatraffic* from *user traffic*. + +- **Metatraffic** carries discovery and endpoint metadata. In this component, + that means SPDP participant announcements plus SEDP publication and + subscription announcements. +- **User traffic** carries application samples. The current ESPP scaffold has a + temporary best-effort `UInt32` user-data path while the standards-based + ROS 2 data plane is still being completed. + +The current `RtpsParticipant` implementation opens three UDP sockets when +`start()` is called: + +1. metatraffic multicast receive on the well-known SPDP multicast port +2. metatraffic unicast receive on the participant-specific discovery port +3. user unicast receive on the participant-specific user-data port + +It then starts a periodic announce task which multicasts SPDP and unicasts SEDP +endpoint announcements to each discovered peer. + +```mermaid +flowchart LR + App["Application code"] --> Participant["RtpsParticipant"] + Participant --> SPDP["SPDP participant DATA"] + Participant --> SEDP["SEDP publication/subscription DATA"] + Participant --> User["User DATA submessages"] + SPDP --> MetaMC["Metatraffic multicast"] + SEDP --> MetaUC["Metatraffic unicast"] + User --> UserUC["User unicast"] + MetaMC --> Peer["Remote participant"] + MetaUC --> Peer + UserUC --> Peer +``` + +## Discovery Flow + +At a high level, discovery proceeds like this: + +```mermaid +sequenceDiagram + participant A as Local participant + participant MC as 239.255.0.1 + participant B as Remote participant + A->>MC: SPDP DATA(participant GUID, locators, enclave, builtin endpoints) + MC-->>B: multicast delivery + B->>MC: SPDP DATA(its participant metadata) + MC-->>A: multicast delivery + A->>B: SEDP publication DATA(topic/type/reliability) + A->>B: SEDP subscription DATA(topic/type/reliability) + B->>A: SEDP publication/subscription DATA + Note over A,B: Matching user-data traffic can then use the user-unicast ports +``` + ## Expected Compatibility The table below is intentionally conservative: **expected** means "this is the diff --git a/components/rtsp/README.md b/components/rtsp/README.md index e6af54e4c..a5408d8eb 100755 --- a/components/rtsp/README.md +++ b/components/rtsp/README.md @@ -12,6 +12,8 @@ performed externally. **Table of Contents** - [RTSP (Real-Time Streaming Protocol) Component](#rtsp-real-time-streaming-protocol-component) + - [How RTSP Works](#how-rtsp-works) + - [Packetization Pipeline](#packetization-pipeline) - [RTSP Client](#rtsp-client) - [RTSP Server](#rtsp-server) - [Packetizers and Depacketizers](#packetizers-and-depacketizers) @@ -20,6 +22,68 @@ performed externally. +## How RTSP Works + +The component uses a split control-plane / media-plane design: + +- **RTSP over TCP** handles session control such as `OPTIONS`, `DESCRIBE`, + `SETUP`, `PLAY`, `PAUSE`, and `TEARDOWN`. +- **SDP** returned from `DESCRIBE` tells the client what tracks exist, how + they are encoded, and which per-track control URLs must be used for + `SETUP`. +- **RTP/UDP** carries encoded media packets after playback starts. +- **RTCP/UDP** sockets are created alongside RTP sockets, but the current ESPP + implementation keeps RTCP support lightweight and does not yet implement a + full control/feedback plane. + +```mermaid +sequenceDiagram + participant App as Application + participant Server as RtspServer / RtspSession + participant Client as RtspClient + App->>Server: add_track() / send_frame() + Client->>Server: OPTIONS + Server-->>Client: 200 OK + Client->>Server: DESCRIBE + Server-->>Client: SDP with session + track control paths + Client->>Server: SETUP(trackID=n, client_port=RTP-RTCP) + Server-->>Client: Session + Transport headers + Client->>Server: PLAY + Server-->>Client: 200 OK + Server-->>Client: RTP/UDP packets for each active track + Client-->>App: on_jpeg_frame() or on_frame(track_id, data) + Client->>Server: TEARDOWN + Server-->>Client: 200 OK +``` + +In ESPP, the server generates one SDP description per session, with one +`m=...` section and one `a=control:.../trackID=N` entry per registered +track. The client parses those lines during `describe()` and then issues +`SETUP` once per discovered track before calling `PLAY`. + +## Packetization Pipeline + +The codec-specific logic is intentionally separated from the RTSP core: + +```mermaid +flowchart LR + Frame["Encoded frame bytes"] --> Packetizer["Codec packetizer"] + Packetizer --> Chunks["RTP payload chunks"] + Chunks --> Header["RtspServer adds RTP headers"] + Header --> Session["RtspSession sends UDP packets"] + Session --> ClientRtp["RtspClient RTP socket"] + ClientRtp --> Depacketizer["Codec depacketizer"] + Depacketizer --> Callback["Application callback"] +``` + +`RtspServer::send_frame(track_id, data)` asks the selected packetizer to split +the encoded frame into MTU-sized chunks, adds RTP headers with track-specific +SSRC and sequence numbers, and leaves the resulting packets queued for active +sessions to transmit. On the client side, `RtspClient::handle_rtp_packet()` +parses the RTP header, uses the payload type to find the matching depacketizer, +and emits a completed frame through either `on_jpeg_frame` or the generic +`on_frame(track_id, data)` callback. + ## RTSP Client The `RtspClient` class connects to an RTSP server and receives media streams From 82bc51ef11478bb821e0b8318920114c91de948f Mon Sep 17 00:00:00 2001 From: William Emfinger Date: Mon, 15 Jun 2026 15:19:52 -0500 Subject: [PATCH 05/32] suppress cppcheck --- components/cdr/include/cdr.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/components/cdr/include/cdr.hpp b/components/cdr/include/cdr.hpp index 42f3ec1f2..c8f47a216 100644 --- a/components/cdr/include/cdr.hpp +++ b/components/cdr/include/cdr.hpp @@ -377,7 +377,7 @@ class CdrReader { requires(std::is_integral_v || std::is_floating_point_v) bool read(T &value) { constexpr size_t alignment = detail::cdr_alignment(); if constexpr (alignment > 1) { - if (!align(alignment)) { + if (!align(alignment)) { // cppcheck-suppress knownConditionTrueFalse valid_ = false; return false; } From 9ab3e86eb7562c320bff6dd8740d9de243206c73 Mon Sep 17 00:00:00 2001 From: William Emfinger Date: Tue, 16 Jun 2026 08:44:05 -0500 Subject: [PATCH 06/32] fix example docs --- components/cdr/example/main/cdr_example.cpp | 4 ++-- components/rtps/example/main/rtps_example.cpp | 2 ++ components/rtps/include/rtps.hpp | 3 +++ 3 files changed, 7 insertions(+), 2 deletions(-) diff --git a/components/cdr/example/main/cdr_example.cpp b/components/cdr/example/main/cdr_example.cpp index 6bc8662cb..4942ce15e 100644 --- a/components/cdr/example/main/cdr_example.cpp +++ b/components/cdr/example/main/cdr_example.cpp @@ -12,12 +12,11 @@ extern "C" void app_main(void) { std::array input_magic{'C', 'D', 'R', '!'}; std::array input_values{10, 20, 30}; + //! [cdr example] espp::CdrWriter writer({ .encapsulation = espp::CdrEncapsulation::CDR_LE, .include_encapsulation = true, }); - - // cdr example writer.write(42); writer.write(3.25f); writer.write_string("hello cdr"); @@ -46,6 +45,7 @@ extern "C" void app_main(void) { bool inline_ok = inline_reader.encapsulation() == espp::CdrEncapsulation::PL_CDR_LE && inline_reader.read_array(decoded_magic) && inline_reader.read_string(decoded_inline_text); + //! [cdr example] if (!ok || !inline_ok) { logger.error("Failed to decode CDR payload"); diff --git a/components/rtps/example/main/rtps_example.cpp b/components/rtps/example/main/rtps_example.cpp index 4786347f7..3b9f55913 100644 --- a/components/rtps/example/main/rtps_example.cpp +++ b/components/rtps/example/main/rtps_example.cpp @@ -70,6 +70,7 @@ bool has_endpoint(std::span endpoint extern "C" void app_main(void) { espp::Logger logger({.tag = "rtps_example", .level = espp::Logger::Verbosity::INFO}); + //! [rtps example] std::string ip_address; espp::WifiSta wifi_sta({.ssid = CONFIG_ESP_WIFI_SSID, .password = CONFIG_ESP_WIFI_PASSWORD, @@ -184,6 +185,7 @@ extern "C" void app_main(void) { logger.error("Failed to start RTPS participant"); return; } + //! [rtps example] #if CONFIG_RTPS_EXAMPLE_ROLE_INITIATOR logger.info("Initiator is waiting for a responder on the same domain/topic prefix..."); diff --git a/components/rtps/include/rtps.hpp b/components/rtps/include/rtps.hpp index 6e8a308da..f796578b5 100644 --- a/components/rtps/include/rtps.hpp +++ b/components/rtps/include/rtps.hpp @@ -19,6 +19,9 @@ namespace espp { /// Cross-platform RTPS protocol foundation built on top of the socket component. +/// +/// \section rtps_ex1 RTPS Example +/// \snippet rtps_example.cpp rtps example class RtpsParticipant : public BaseComponent { public: /// @brief Delivery semantics advertised for a writer or reader endpoint. From 95979f348d1723eecf69c60ca19bdb66929829ce Mon Sep 17 00:00:00 2001 From: William Emfinger Date: Tue, 16 Jun 2026 13:10:07 -0500 Subject: [PATCH 07/32] fix some bugs in data parsing so that examples properly communicate. add initial support for user data multicast --- components/rtps/README.md | 2 + components/rtps/example/README.md | 18 +- .../rtps/example/main/Kconfig.projbuild | 28 +- components/rtps/example/main/rtps_example.cpp | 28 +- components/rtps/include/rtps.hpp | 29 +- components/rtps/src/rtps.cpp | 260 +++++++++++++++--- doc/en/rtps.rst | 6 + python/README.md | 7 +- python/rtps_host.py | 97 ++++++- 9 files changed, 417 insertions(+), 58 deletions(-) diff --git a/components/rtps/README.md b/components/rtps/README.md index c9babda33..c07cd98a0 100644 --- a/components/rtps/README.md +++ b/components/rtps/README.md @@ -15,6 +15,7 @@ This component now includes the first real RTPS discovery slice: - SEDP publication and subscription announcements for local endpoints - parsing and tracking of discovered remote participants, writers, and readers - integration with the shared `cdr` component for CDR/PL_CDR payload handling +- optional best-effort user-data multicast transport, including endpoint-specific groups The long-term goal for this component is DDS/RTPS interoperability with ROS 2 nodes, including best-effort and reliable user-data flows. Discovery is now @@ -100,6 +101,7 @@ against every stack". | SEDP publication / subscription announce send/receive | **Implemented** | Local endpoints are announced and remote endpoints are cached. | | Participant / endpoint discovery callbacks | **Implemented** | Exposed through `on_participant_discovered` and `on_endpoint_discovered`. | | Temporary `UInt32` user-data path | **Implemented** | Uses the current ESPP-specific `ESPPDATA` payload, not a standards-based DDS sample representation. | +| Best-effort user-data multicast transport | **Implemented** | Supports shared participant-level multicast or endpoint-specific multicast locators advertised in SEDP; local readers only join the multicast groups configured for their topics. | | QoS fields emitted in discovery | **Partial** | Reliability, durability, liveliness, and history parameters are advertised in SEDP. | | QoS matching / policy enforcement | **Not implemented** | Remote QoS is parsed, but full writer/reader matching logic is still missing. | | Standards-based DDS user-data serialization | **Not implemented** | The current data path is a temporary ESPP scaffold for `std_msgs/msg/UInt32`. | diff --git a/components/rtps/example/README.md b/components/rtps/example/README.md index 11f5259ee..0c2183c98 100644 --- a/components/rtps/example/README.md +++ b/components/rtps/example/README.md @@ -26,7 +26,18 @@ For both boards: 1. Set the same `RTPS domain ID`, `Topic prefix`, `WiFi SSID`, and `WiFi password`. 2. Give each board a unique `RTPS participant ID`. -3. Optionally set distinct `Participant node name` values to make discovery logs easier to read. +3. Give each board a distinct `Participant node name` or keep the role-specific defaults. +4. If you want to exercise multicast user data, enable `Use best-effort user-data multicast` + on both boards and keep the same request/response multicast groups on each node. + +Fresh example configurations now default to: + +* initiator: participant ID `1`, node name `espp_rtps_initiator` +* responder: participant ID `2`, node name `espp_rtps_responder` + +If you are reusing an older build directory or `sdkconfig`, rerun `idf.py menuconfig` +or delete the stale generated config so the old shared defaults (`participant ID = 1`, +`node name = espp_rtps_node`) do not persist on both boards. For one board only: @@ -59,3 +70,8 @@ Expected signs of success: * both boards log RTPS participant and endpoint discovery * the initiator logs `Published request N` followed by `Received response N` * the responder logs `Received request N, sending response` + +When multicast user data is enabled, discovery still uses the normal RTPS +metatraffic sockets, but request samples are published to the configured request +multicast group and response samples are published to the configured response +multicast group. Each node only joins the group for the topic it subscribes to. diff --git a/components/rtps/example/main/Kconfig.projbuild b/components/rtps/example/main/Kconfig.projbuild index ff2656842..cdeb799f7 100644 --- a/components/rtps/example/main/Kconfig.projbuild +++ b/components/rtps/example/main/Kconfig.projbuild @@ -23,7 +23,8 @@ menu "RTPS Example Configuration" config RTPS_EXAMPLE_NODE_NAME string "Participant node name" - default "espp_rtps_node" + default "espp_rtps_initiator" if RTPS_EXAMPLE_ROLE_INITIATOR + default "espp_rtps_responder" if RTPS_EXAMPLE_ROLE_RESPONDER help Logical RTPS participant name announced during discovery. @@ -37,7 +38,8 @@ menu "RTPS Example Configuration" config RTPS_EXAMPLE_PARTICIPANT_ID int "RTPS participant ID" range 0 119 - default 1 + default 1 if RTPS_EXAMPLE_ROLE_INITIATOR + default 2 if RTPS_EXAMPLE_ROLE_RESPONDER help Each board should use a unique participant ID within the same domain. @@ -63,6 +65,28 @@ menu "RTPS Example Configuration" Period between request messages sent by the initiator after a responder has been discovered. + config RTPS_EXAMPLE_USE_USER_MULTICAST + bool "Use best-effort user-data multicast" + default n + help + If enabled, the example advertises and uses topic-specific + multicast groups for the request and response topics instead of + per-participant unicast delivery. + + config RTPS_EXAMPLE_REQUEST_MULTICAST_GROUP + string "Request topic multicast group" + default "239.255.0.11" + depends on RTPS_EXAMPLE_USE_USER_MULTICAST + help + IPv4 multicast group used for the /request topic. + + config RTPS_EXAMPLE_RESPONSE_MULTICAST_GROUP + string "Response topic multicast group" + default "239.255.0.12" + depends on RTPS_EXAMPLE_USE_USER_MULTICAST + help + IPv4 multicast group used for the /response topic. + config ESP_WIFI_SSID string "WiFi SSID" default "" diff --git a/components/rtps/example/main/rtps_example.cpp b/components/rtps/example/main/rtps_example.cpp index 3b9f55913..6cea5bdc4 100644 --- a/components/rtps/example/main/rtps_example.cpp +++ b/components/rtps/example/main/rtps_example.cpp @@ -58,8 +58,8 @@ bool run_local_protocol_checks(espp::Logger &logger, const espp::RtpsParticipant return true; } -bool has_endpoint(std::span endpoints, - std::string_view topic_name, bool is_reader) { +[[maybe_unused]] bool has_endpoint(std::span endpoints, + std::string_view topic_name, bool is_reader) { return std::any_of(endpoints.begin(), endpoints.end(), [topic_name, is_reader](const auto &endpoint) { return endpoint.topic_name == topic_name && endpoint.is_reader == is_reader; @@ -92,6 +92,22 @@ extern "C" void app_main(void) { const std::string topic_prefix = CONFIG_RTPS_EXAMPLE_TOPIC_PREFIX; const std::string request_topic = topic_prefix + "/request"; const std::string response_topic = topic_prefix + "/response"; +#if CONFIG_RTPS_EXAMPLE_USE_USER_MULTICAST +#if defined(CONFIG_RTPS_EXAMPLE_REQUEST_MULTICAST_GROUP) && \ + defined(CONFIG_RTPS_EXAMPLE_RESPONSE_MULTICAST_GROUP) + const std::string request_multicast_group = CONFIG_RTPS_EXAMPLE_REQUEST_MULTICAST_GROUP; + const std::string response_multicast_group = CONFIG_RTPS_EXAMPLE_RESPONSE_MULTICAST_GROUP; +#elif defined(CONFIG_RTPS_EXAMPLE_USER_MULTICAST_GROUP) + const std::string request_multicast_group = CONFIG_RTPS_EXAMPLE_USER_MULTICAST_GROUP; + const std::string response_multicast_group = CONFIG_RTPS_EXAMPLE_USER_MULTICAST_GROUP; +#else + const std::string request_multicast_group = "239.255.0.11"; + const std::string response_multicast_group = "239.255.0.12"; +#endif +#else + const std::string request_multicast_group; + const std::string response_multicast_group; +#endif std::atomic request_count{0}; std::atomic response_count{0}; @@ -123,12 +139,14 @@ extern "C" void app_main(void) { .topic_name = request_topic, .type_name = std::string(kTypeName), .reliability = espp::RtpsParticipant::ReliabilityKind::BEST_EFFORT, + .multicast_group = request_multicast_group, .entity_index = 0, }); participant.add_reader({ .topic_name = response_topic, .type_name = std::string(kTypeName), .reliability = espp::RtpsParticipant::ReliabilityKind::BEST_EFFORT, + .multicast_group = response_multicast_group, .entity_index = 0, .on_uint32_sample = [&logger, &response_count, &last_sent_request](uint32_t value) { @@ -142,12 +160,14 @@ extern "C" void app_main(void) { .topic_name = response_topic, .type_name = std::string(kTypeName), .reliability = espp::RtpsParticipant::ReliabilityKind::BEST_EFFORT, + .multicast_group = response_multicast_group, .entity_index = 0, }); participant.add_reader({ .topic_name = request_topic, .type_name = std::string(kTypeName), .reliability = espp::RtpsParticipant::ReliabilityKind::BEST_EFFORT, + .multicast_group = request_multicast_group, .entity_index = 0, .on_uint32_sample = [&logger, &request_count, &response_topic, &participant_ptr](uint32_t value) { @@ -176,6 +196,10 @@ extern "C" void app_main(void) { logger.info("Request topic: {}, Response topic: {}", request_topic, response_topic); logger.info("Ports: meta mc={}, meta uc={}, user mc={}, user uc={}", ports.metatraffic_multicast, ports.metatraffic_unicast, ports.user_multicast, ports.user_unicast); +#if CONFIG_RTPS_EXAMPLE_USE_USER_MULTICAST + logger.info("User-data multicast enabled: request group {}, response group {}", + request_multicast_group, response_multicast_group); +#endif if (!run_local_protocol_checks(logger, participant)) { return; diff --git a/components/rtps/include/rtps.hpp b/components/rtps/include/rtps.hpp index f796578b5..e861cfab8 100644 --- a/components/rtps/include/rtps.hpp +++ b/components/rtps/include/rtps.hpp @@ -166,7 +166,9 @@ class RtpsParticipant : public BaseComponent { std::string type_name{"std_msgs/msg/UInt32"}; ///< Type name advertised through SEDP. ReliabilityKind reliability{ ReliabilityKind::BEST_EFFORT}; ///< Reliability QoS advertised for the writer. - uint32_t entity_index{0}; ///< Local entity slot used to derive the RTPS entity ID. + std::string multicast_group{}; ///< Optional multicast group advertised for this writer and used + ///< by `publish_uint32()` when set. + uint32_t entity_index{0}; ///< Local entity slot used to derive the RTPS entity ID. }; /// @brief Configuration for a locally advertised reader endpoint. @@ -175,7 +177,9 @@ class RtpsParticipant : public BaseComponent { std::string type_name{"std_msgs/msg/UInt32"}; ///< Type name advertised through SEDP. ReliabilityKind reliability{ ReliabilityKind::BEST_EFFORT}; ///< Reliability QoS advertised for the reader. - uint32_t entity_index{0}; ///< Local entity slot used to derive the RTPS entity ID. + std::string multicast_group{}; ///< Optional multicast group advertised for this reader and + ///< joined on the standard RTPS user-multicast port when set. + uint32_t entity_index{0}; ///< Local entity slot used to derive the RTPS entity ID. std::function on_uint32_sample{ nullptr}; ///< Callback invoked when a matching temporary UInt32 sample is received. }; @@ -201,6 +205,8 @@ class RtpsParticipant : public BaseComponent { bool is_reader{false}; ///< True for discovered readers, false for discovered writers. bool expects_inline_qos{false}; ///< Whether the remote endpoint requested inline QoS. Locator unicast_locator{}; ///< Preferred unicast locator advertised by the endpoint. + std::vector multicast_locators{}; ///< Multicast locators advertised by the endpoint + ///< for user-data traffic. }; /// @brief Top-level participant configuration. @@ -213,6 +219,10 @@ class RtpsParticipant : public BaseComponent { "127.0.0.1"}; ///< IPv4 address advertised to peers for unicast traffic. std::string metatraffic_multicast_group{ "239.255.0.1"}; ///< Multicast group used for RTPS metatraffic discovery. + std::string user_multicast_group{ + "239.255.0.1"}; ///< Multicast group used for best-effort user-data multicast when enabled. + bool use_multicast_for_user_data{false}; ///< If true, join the user multicast group and publish + ///< temporary user-data samples via multicast. Task::BaseConfig receive_task_config{ .name = "RtpsRx", .stack_size_bytes = 6 * 1024}; ///< Base task configuration for receive sockets. @@ -317,14 +327,14 @@ class RtpsParticipant : public BaseComponent { std::vector build_sedp_subscription_message(const ReaderConfig &reader_config) const; /// @brief Build a temporary ESPP UInt32 user-data message. - /// @param topic_name Topic name to embed in the message payload. + /// @param writer_config Local writer configuration used for the topic and writer entity ID. /// @param value UInt32 sample value to serialize. /// @param reliability Reliability flag to encode in the temporary payload header. /// @return Serialized RTPS DATA message containing the temporary ESPP UInt32 payload. - std::vector build_uint32_data_message(std::string_view topic_name, uint32_t value, + std::vector build_uint32_data_message(const WriterConfig &writer_config, uint32_t value, ReliabilityKind reliability) const; - /// @brief Publish a temporary ESPP UInt32 sample to discovered participants. + /// @brief Publish a temporary ESPP UInt32 sample using the configured user-data transport. /// @param topic_name Topic name to publish on. Must match a registered local writer. /// @param value UInt32 sample value to send. /// @return True if at least one send call succeeded, false otherwise. @@ -347,8 +357,16 @@ class RtpsParticipant : public BaseComponent { static PortMapping compute_port_mapping(uint16_t domain_id, uint16_t participant_id); private: + struct UserMulticastReceiver { + std::string multicast_group{}; + std::unique_ptr socket{}; + }; + bool handle_metatraffic_message(std::vector &data, const Socket::Info &sender); bool handle_user_message(std::vector &data, const Socket::Info &sender); + bool ensure_user_multicast_receivers_started(); + std::vector + build_user_send_configs(std::string_view topic_name, const WriterConfig &writer_config) const; bool send_spdp_announce_now(); bool send_sedp_announcements_to(const ParticipantProxy &participant); bool send_discovery_now(); @@ -360,6 +378,7 @@ class RtpsParticipant : public BaseComponent { std::unique_ptr metatraffic_multicast_receiver_; std::unique_ptr metatraffic_unicast_receiver_; + std::vector user_multicast_receivers_; std::unique_ptr user_unicast_receiver_; std::unique_ptr announce_task_; diff --git a/components/rtps/src/rtps.cpp b/components/rtps/src/rtps.cpp index 04ca02097..83bf00d8d 100644 --- a/components/rtps/src/rtps.cpp +++ b/components/rtps/src/rtps.cpp @@ -474,6 +474,17 @@ std::optional find_parameter(std::span param return *iterator; } +std::vector find_parameters(std::span parameters, + ParameterId id) { + std::vector matches; + for (const auto ¶meter : parameters) { + if (parameter.id == id) { + matches.push_back(parameter); + } + } + return matches; +} + std::optional parse_guid(std::span value) { if (value.size() != 16) { return std::nullopt; @@ -506,11 +517,15 @@ std::optional parse_cdr_string(std::span value) { if (!reader.valid()) { return std::nullopt; } - std::string text; - if (!reader.read_string(text)) { + uint32_t length = 0; + if (!reader.read(length) || length == 0) { + return std::nullopt; + } + auto text_bytes = reader.read_span(length); + if (text_bytes.size() != length || text_bytes.back() != 0) { return std::nullopt; } - return text; + return std::string(reinterpret_cast(text_bytes.data()), text_bytes.size() - 1); } std::optional> parse_octet_sequence(std::span value) { @@ -546,6 +561,10 @@ std::optional parse_locator(std::span parse_reliability(std::span value) { auto maybe_kind = parse_u32_le(value); @@ -817,6 +836,11 @@ bool RtpsParticipant::start() { return false; } + if (!ensure_user_multicast_receivers_started()) { + stop(); + return false; + } + announce_task_ = Task::make_unique({ .callback = [this](std::mutex &mutex, std::condition_variable &cv, bool ¬ified) -> bool { send_discovery_now(); @@ -848,6 +872,12 @@ void RtpsParticipant::stop() { metatraffic_unicast_receiver_->stop_receiving(); metatraffic_unicast_receiver_.reset(); } + for (auto &receiver : user_multicast_receivers_) { + if (receiver.socket) { + receiver.socket->stop_receiving(); + } + } + user_multicast_receivers_.clear(); if (user_unicast_receiver_) { user_unicast_receiver_->stop_receiving(); user_unicast_receiver_.reset(); @@ -863,8 +893,15 @@ bool RtpsParticipant::add_writer(const WriterConfig &writer_config) { } bool RtpsParticipant::add_reader(const ReaderConfig &reader_config) { - std::lock_guard lock(mutex_); - readers_.push_back(reader_config); + { + std::lock_guard lock(mutex_); + readers_.push_back(reader_config); + } + if (started_.load() && !reader_config.multicast_group.empty() && + !ensure_user_multicast_receivers_started()) { + logger_.error("Failed to start multicast receiver for topic '{}'", reader_config.topic_name); + return false; + } return true; } @@ -929,9 +966,10 @@ std::vector RtpsParticipant::build_spdp_announce_message() const { Locator::udp_v4(config_.advertised_address, ports().metatraffic_unicast)); append_parameter_locator(parameters, ParameterId::PID_DEFAULT_UNICAST_LOCATOR, Locator::udp_v4(config_.advertised_address, ports().user_unicast)); - append_parameter_locator( - parameters, ParameterId::PID_DEFAULT_MULTICAST_LOCATOR, - Locator::udp_v4(config_.metatraffic_multicast_group, ports().user_multicast)); + if (config_.use_multicast_for_user_data) { + append_parameter_locator(parameters, ParameterId::PID_DEFAULT_MULTICAST_LOCATOR, + Locator::udp_v4(config_.user_multicast_group, ports().user_multicast)); + } append_parameter_duration(parameters, ParameterId::PID_PARTICIPANT_LEASE_DURATION, kDefaultLeaseDurationSeconds, kDefaultLeaseDurationNanoseconds); append_parameter_u32(parameters, ParameterId::PID_BUILTIN_ENDPOINT_SET, kBuiltinEndpointSet); @@ -956,6 +994,11 @@ RtpsParticipant::build_sedp_publication_message(const WriterConfig &writer_confi append_parameter_guid(parameters, ParameterId::PID_ENDPOINT_GUID, guid); append_parameter_locator(parameters, ParameterId::PID_UNICAST_LOCATOR, Locator::udp_v4(config_.advertised_address, ports().user_unicast)); + if (!writer_config.multicast_group.empty()) { + append_parameter_locator( + parameters, ParameterId::PID_MULTICAST_LOCATOR, + Locator::udp_v4(writer_config.multicast_group, ports().user_multicast)); + } append_parameter_guid(parameters, ParameterId::PID_PARTICIPANT_GUID, participant_guid()); append_parameter_string_cdr(parameters, ParameterId::PID_TOPIC_NAME, writer_config.topic_name); append_parameter_string_cdr(parameters, ParameterId::PID_TYPE_NAME, writer_config.type_name); @@ -984,6 +1027,11 @@ RtpsParticipant::build_sedp_subscription_message(const ReaderConfig &reader_conf append_parameter_guid(parameters, ParameterId::PID_ENDPOINT_GUID, guid); append_parameter_locator(parameters, ParameterId::PID_UNICAST_LOCATOR, Locator::udp_v4(config_.advertised_address, ports().user_unicast)); + if (!reader_config.multicast_group.empty()) { + append_parameter_locator( + parameters, ParameterId::PID_MULTICAST_LOCATOR, + Locator::udp_v4(reader_config.multicast_group, ports().user_multicast)); + } append_parameter_bool(parameters, ParameterId::PID_EXPECTS_INLINE_QOS, false); append_parameter_guid(parameters, ParameterId::PID_PARTICIPANT_GUID, participant_guid()); append_parameter_string_cdr(parameters, ParameterId::PID_TOPIC_NAME, reader_config.topic_name); @@ -1004,19 +1052,19 @@ RtpsParticipant::build_sedp_subscription_message(const ReaderConfig &reader_conf .serialize(); } -std::vector RtpsParticipant::build_uint32_data_message(std::string_view topic_name, +std::vector RtpsParticipant::build_uint32_data_message(const WriterConfig &writer_config, uint32_t value, ReliabilityKind reliability) const { ByteWriter payload_writer; payload_writer.append_chars(kUserDataMagic); payload_writer.append_u8(kUserDataVersion); payload_writer.append_u8(static_cast(reliability)); - append_string(payload_writer, topic_name); + append_string(payload_writer, writer_config.topic_name); auto cdr = serialize_uint32_cdr(value); payload_writer.append_u16_le(static_cast(cdr.size())); payload_writer.append_bytes(cdr); - auto guid = writers_.empty() ? writer_guid(0) : writer_guid(writers_.front().entity_index); + auto guid = writer_guid(writer_config.entity_index); return build_message(guid_prefix_, {.value = kEntityIdUnknown}, guid.entity_id, 1, payload_writer.take()) .serialize(); @@ -1043,23 +1091,20 @@ bool RtpsParticipant::publish_uint32(std::string_view topic_name, uint32_t value auto encoded_reliability = writer_config.reliability == ReliabilityKind::RELIABLE ? ReliabilityKind::BEST_EFFORT : writer_config.reliability; - auto payload = build_uint32_data_message(topic_name, value, encoded_reliability); - auto participants = discovered_participants(); - if (participants.empty()) { - logger_.warn("No discovered participants available for topic '{}'", topic_name); + auto payload = build_uint32_data_message(writer_config, value, encoded_reliability); + + if (!user_unicast_receiver_) { return false; } - if (!user_unicast_receiver_) { + auto send_configs = build_user_send_configs(topic_name, writer_config); + if (send_configs.empty()) { + logger_.warn("No send destinations available for topic '{}'", topic_name); return false; } bool sent = false; - for (const auto &participant : participants) { - auto send_config = UdpSocket::SendConfig{ - .ip_address = participant.address, - .port = participant.ports.user_unicast, - }; + for (const auto &send_config : send_configs) { sent = user_unicast_receiver_->send(payload, send_config) || sent; } return sent; @@ -1192,12 +1237,12 @@ bool RtpsParticipant::handle_metatraffic_message(std::vector &data, callback = config_.on_participant_discovered; } - logger_.info("SPDP discovered participant '{}' at {} (meta {}, user {})", - participant.name.empty() ? participant.guid_prefix.to_string() - : participant.name, - participant.address, participant.ports.metatraffic_unicast, - participant.ports.user_unicast); if (is_new_participant) { + logger_.info("SPDP discovered participant '{}' at {} (meta {}, user {})", + participant.name.empty() ? participant.guid_prefix.to_string() + : participant.name, + participant.address, participant.ports.metatraffic_unicast, + participant.ports.user_unicast); send_sedp_announcements_to(participant); if (callback) { callback(participant); @@ -1254,6 +1299,12 @@ bool RtpsParticipant::handle_metatraffic_message(std::vector &data, endpoint.unicast_locator = *maybe_locator; } } + for (const auto &locator_parameter : + find_parameters(parameters, ParameterId::PID_MULTICAST_LOCATOR)) { + if (auto maybe_locator = parse_locator(locator_parameter.value)) { + endpoint.multicast_locators.push_back(*maybe_locator); + } + } if (auto maybe_reliability_parameter = find_parameter(parameters, ParameterId::PID_RELIABILITY)) { if (auto maybe_reliability = parse_reliability(maybe_reliability_parameter->value)) { @@ -1284,11 +1335,13 @@ bool RtpsParticipant::handle_metatraffic_message(std::vector &data, endpoint_callback = config_.on_endpoint_discovered; } - logger_.info("SEDP discovered {} '{}' [{}] from participant {}", - endpoint.is_reader ? "reader" : "writer", endpoint.topic_name, endpoint.type_name, - endpoint.participant_guid.to_string()); - if (is_new_endpoint && endpoint_callback) { - endpoint_callback(endpoint); + if (is_new_endpoint) { + logger_.info("SEDP discovered {} '{}' [{}] from participant {}", + endpoint.is_reader ? "reader" : "writer", endpoint.topic_name, + endpoint.type_name, endpoint.participant_guid.to_string()); + if (endpoint_callback) { + endpoint_callback(endpoint); + } } } return false; @@ -1299,16 +1352,20 @@ bool RtpsParticipant::handle_user_message(std::vector &data, const Sock if (!message) { return false; } + if (message->header.guid_prefix == guid_prefix_) { + return false; + } for (const auto &submessage : message->submessages) { - if (submessage.kind != SubmessageKind::DATA || - submessage.payload.size() < kUserDataMagic.size() + 2 || - !std::equal(kUserDataMagic.begin(), kUserDataMagic.end(), submessage.payload.begin())) { + bool valid_data = false; + auto data_view = parse_data_submessage(submessage, valid_data); + if (!valid_data || data_view.serialized_payload.size() < kUserDataMagic.size() + 2 || + !std::equal(kUserDataMagic.begin(), kUserDataMagic.end(), + data_view.serialized_payload.begin())) { continue; } - ByteReader reader( - std::span{submessage.payload.data(), submessage.payload.size()}); + ByteReader reader(data_view.serialized_payload); std::array magic{}; uint8_t version = 0; uint8_t reliability = 0; @@ -1349,6 +1406,137 @@ bool RtpsParticipant::handle_user_message(std::vector &data, const Sock return false; } +bool RtpsParticipant::ensure_user_multicast_receivers_started() { + if (!started_.load()) { + return true; + } + + std::vector desired_groups; + if (config_.use_multicast_for_user_data && !config_.user_multicast_group.empty()) { + desired_groups.push_back(config_.user_multicast_group); + } + { + std::lock_guard lock(mutex_); + for (const auto &reader_config : readers_) { + if (!reader_config.multicast_group.empty() && + std::find(desired_groups.begin(), desired_groups.end(), reader_config.multicast_group) == + desired_groups.end()) { + desired_groups.push_back(reader_config.multicast_group); + } + } + } + + auto port_mapping = ports(); + for (const auto &group : desired_groups) { + auto existing = + std::find_if(user_multicast_receivers_.begin(), user_multicast_receivers_.end(), + [&group](const auto &receiver) { return receiver.multicast_group == group; }); + if (existing != user_multicast_receivers_.end()) { + continue; + } + + auto socket = std::make_unique(UdpSocket::Config{.log_level = get_log_level()}); + auto task_config = config_.receive_task_config; + task_config.name = fmt::format("{}_user_mc_{}", config_.receive_task_config.name, + user_multicast_receivers_.size()); + auto receive_config = UdpSocket::ReceiveConfig{ + .port = port_mapping.user_multicast, + .buffer_size = 4096, + .is_multicast_endpoint = true, + .multicast_group = group, + .on_receive_callback = [this](auto &data, + const auto &sender) -> std::optional> { + handle_user_message(data, sender); + return std::nullopt; + }, + }; + if (!socket->start_receiving(task_config, receive_config)) { + logger_.error("Failed to start user multicast receiver for group {}", group); + return false; + } + user_multicast_receivers_.push_back({ + .multicast_group = group, + .socket = std::move(socket), + }); + } + return true; +} + +std::vector +RtpsParticipant::build_user_send_configs(std::string_view topic_name, + const WriterConfig &writer_config) const { + std::vector send_configs; + auto add_send_config = [&send_configs](std::string ip_address, uint16_t port, bool is_multicast) { + if (ip_address.empty() || port == 0) { + return; + } + auto existing = + std::find_if(send_configs.begin(), send_configs.end(), [&](const auto &send_config) { + return send_config.ip_address == ip_address && send_config.port == port && + send_config.is_multicast_endpoint == is_multicast; + }); + if (existing == send_configs.end()) { + send_configs.push_back({ + .ip_address = std::move(ip_address), + .port = port, + .is_multicast_endpoint = is_multicast, + }); + } + }; + + if (!writer_config.multicast_group.empty()) { + add_send_config(writer_config.multicast_group, ports().user_multicast, true); + return send_configs; + } + + if (config_.use_multicast_for_user_data) { + add_send_config(config_.user_multicast_group, ports().user_multicast, true); + return send_configs; + } + + std::vector remote_readers; + std::vector participants; + { + std::lock_guard lock(mutex_); + remote_readers = discovered_readers_; + participants = discovered_participants_; + } + + for (const auto &reader : remote_readers) { + if (!reader.is_reader || reader.topic_name != topic_name) { + continue; + } + + bool used_multicast = false; + for (const auto &locator : reader.multicast_locators) { + if (!has_valid_locator(locator)) { + continue; + } + add_send_config(locator.address_string(), static_cast(locator.port), true); + used_multicast = true; + } + if (used_multicast) { + continue; + } + + if (has_valid_locator(reader.unicast_locator)) { + add_send_config(reader.unicast_locator.address_string(), + static_cast(reader.unicast_locator.port), false); + continue; + } + + auto participant = + std::find_if(participants.begin(), participants.end(), [&](const auto &proxy) { + return proxy.guid_prefix == reader.participant_guid.prefix; + }); + if (participant != participants.end()) { + add_send_config(participant->address, participant->ports.user_unicast, false); + } + } + + return send_configs; +} + bool RtpsParticipant::send_spdp_announce_now() { if (!metatraffic_unicast_receiver_) { return false; diff --git a/doc/en/rtps.rst b/doc/en/rtps.rst index a5da1cc60..1762e4485 100644 --- a/doc/en/rtps.rst +++ b/doc/en/rtps.rst @@ -17,6 +17,7 @@ This version now implements the first RTPS discovery layer on top of the ESPP - integration with the shared ``cdr`` component for CDR/PL_CDR payload handling - a participant transport layer that uses ``UdpSocket`` for metatraffic and user-traffic channels +- optional best-effort user-data multicast transport, including endpoint-specific groups The long-term target is interoperability with ROS 2 nodes over DDS/RTPS, including best-effort and reliable data flows. Discovery messages are now @@ -197,6 +198,11 @@ Feature Status - **Implemented** - Uses the current ESPP-specific ``ESPPDATA`` payload, not a standards-based DDS sample representation. + * - Best-effort user-data multicast transport + - **Implemented** + - Supports shared participant-level multicast or endpoint-specific + multicast locators advertised in SEDP; local readers only join the + multicast groups configured for their topics. * - QoS fields emitted in discovery - **Partial** - Reliability, durability, liveliness, and history parameters are diff --git a/python/README.md b/python/README.md index 429c5653f..9ea454864 100644 --- a/python/README.md +++ b/python/README.md @@ -50,7 +50,8 @@ This section gives a brief overview of what the scripts in this folder do. - `rtps_host.py`: A pure-stdlib host-side RTPS harness for discovering an ESPP `RtpsParticipant`, printing SPDP/SEDP metadata, and optionally publishing or receiving the current temporary `UInt32` user-data payloads without needing - Python bindings. + Python bindings. It now follows endpoint-advertised user-data multicast + locators, joining matching subscribed-topic multicast groups dynamically. - `cobs_demo.py`: Demonstration of ESPP COBS functionality with native Python data types. Shows ESPP encoding/decoding, cross-library compatibility with the cobs-python library, and practical usage examples. Includes design differences explanation and validation @@ -130,6 +131,10 @@ For the default ESP RTPS example, the host harness now defaults to the python3 rtps_host.py --advertised-address 192.168.1.50 ``` +When the ESP RTPS example is configured for per-topic multicast, the host +harness will automatically join the discovered request-topic multicast group and +send response samples using the discovered response-reader locators. + To act as the initiator instead, swap the topics and enable periodic publishing: ```console diff --git a/python/rtps_host.py b/python/rtps_host.py index 8c30e92ec..0aabadb38 100644 --- a/python/rtps_host.py +++ b/python/rtps_host.py @@ -25,7 +25,7 @@ import sys import time from dataclasses import dataclass -from typing import Dict, Iterable, List, Optional +from typing import Dict, Iterable, List, Optional, Set, Tuple RTPS_MAGIC = b"RTPS" @@ -86,6 +86,7 @@ PID_LIVELINESS = 0x001B PID_DURABILITY = 0x001D PID_USER_DATA = 0x002C +PID_MULTICAST_LOCATOR = 0x0030 PID_UNICAST_LOCATOR = 0x002F PID_DEFAULT_UNICAST_LOCATOR = 0x0031 PID_METATRAFFIC_UNICAST_LOCATOR = 0x0032 @@ -139,6 +140,7 @@ class EndpointProxy: expects_inline_qos: bool unicast_address: str unicast_port: int + multicast_locators: List[Tuple[str, int]] @dataclass @@ -375,6 +377,10 @@ def find_parameter(parameters: Iterable[tuple[int, bytes]], pid: int) -> Optiona return None +def find_parameters(parameters: Iterable[tuple[int, bytes]], pid: int) -> List[bytes]: + return [candidate_value for candidate_pid, candidate_value in parameters if candidate_pid == pid] + + def parse_guid(value: Optional[bytes]) -> Optional[bytes]: if value is None or len(value) != 16: return None @@ -465,6 +471,7 @@ def __init__(self, args: argparse.Namespace) -> None: self.discovered_participants: Dict[bytes, ParticipantProxy] = {} self.discovered_writers: Dict[bytes, EndpointProxy] = {} self.discovered_readers: Dict[bytes, EndpointProxy] = {} + self.joined_user_multicast_groups: Set[str] = set() self.local_writers = [ WriterConfig( @@ -487,7 +494,9 @@ def __init__(self, args: argparse.Namespace) -> None: self.metatraffic_multicast_sock = self._create_metatraffic_multicast_socket() self.metatraffic_unicast_sock = self._create_bound_udp_socket(self.ports.metatraffic_unicast) self.user_unicast_sock = self._create_bound_udp_socket(self.ports.user_unicast) + self.user_multicast_sock = self._create_user_multicast_socket() self._configure_multicast_sender(self.metatraffic_unicast_sock) + self._configure_multicast_sender(self.user_unicast_sock) self.next_discovery_send = 0.0 self.next_publish_send = 0.0 @@ -529,6 +538,27 @@ def _create_metatraffic_multicast_socket(self) -> socket.socket: sock.setblocking(False) return sock + def _create_user_multicast_socket(self) -> socket.socket: + sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM, socket.IPPROTO_UDP) + sock.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1) + if hasattr(socket, "SO_REUSEPORT"): + try: + sock.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEPORT, 1) + except OSError: + pass + sock.bind((self.args.bind_address, self.ports.user_multicast)) + sock.setblocking(False) + return sock + + def _join_user_multicast_group(self, group: str) -> None: + if group in self.joined_user_multicast_groups: + return + interface_ip = self.args.multicast_interface or self.args.advertised_address + membership = socket.inet_aton(group) + socket.inet_aton(interface_ip) + self.user_multicast_sock.setsockopt(socket.IPPROTO_IP, socket.IP_ADD_MEMBERSHIP, membership) + self.joined_user_multicast_groups.add(group) + log(f"[multicast] joined user-data group {group}:{self.ports.user_multicast}") + def _configure_multicast_sender(self, sock: socket.socket) -> None: sock.setsockopt(socket.IPPROTO_IP, socket.IP_MULTICAST_TTL, 1) sock.setsockopt(socket.IPPROTO_IP, socket.IP_MULTICAST_LOOP, 1) @@ -706,18 +736,42 @@ def publish_now(self) -> None: else: log( f"[publish] sent {self.args.publish_value} on '{self.local_writers[0].topic_name}' " - f"to {len(self.discovered_participants)} discovered participant(s)" + f"using {len(self._build_user_targets(self.local_writers[0]))} discovered target(s)" ) + def _build_user_targets(self, writer: WriterConfig) -> List[Tuple[str, int]]: + targets: List[Tuple[str, int]] = [] + for reader in self.discovered_readers.values(): + if reader.topic_name != writer.topic_name: + continue + if reader.multicast_locators: + for multicast_address, multicast_port in reader.multicast_locators: + target = (multicast_address, multicast_port) + if target not in targets: + targets.append(target) + continue + if reader.unicast_port > 0 and reader.unicast_address: + target = (reader.unicast_address, reader.unicast_port) + if target not in targets: + targets.append(target) + if targets: + return targets + for participant in self.discovered_participants.values(): + target = (participant.address, participant.ports.user_unicast) + if participant.address and participant.ports.user_unicast > 0 and target not in targets: + targets.append(target) + return targets + def _publish_value(self, writer: WriterConfig, value: int, target: Optional[tuple[str, int]] = None) -> bool: payload = self.build_uint32_data_message(writer, value) if target is not None: self.user_unicast_sock.sendto(payload, target) return True - if not self.discovered_participants: + targets = self._build_user_targets(writer) + if not targets: return False - for participant in self.discovered_participants.values(): - self.user_unicast_sock.sendto(payload, (participant.address, participant.ports.user_unicast)) + for destination in targets: + self.user_unicast_sock.sendto(payload, destination) return True def handle_metatraffic_packet(self, packet: bytes, sender_ip: str) -> None: @@ -779,6 +833,15 @@ def _handle_sedp(self, parameters: List[tuple[int, bytes]], sender_ip: str, is_r participant_guid = endpoint_guid[:12] + PARTICIPANT_ENTITY_ID endpoint_ip, endpoint_port = parse_locator(find_parameter(parameters, PID_UNICAST_LOCATOR)) + multicast_locators = [ + parse_locator(value) + for value in find_parameters(parameters, PID_MULTICAST_LOCATOR) + ] + multicast_locators = [ + (multicast_address, multicast_port) + for multicast_address, multicast_port in multicast_locators + if multicast_address != "0.0.0.0" and multicast_port > 0 + ] endpoint = EndpointProxy( guid=endpoint_guid, participant_guid=participant_guid, @@ -789,10 +852,17 @@ def _handle_sedp(self, parameters: List[tuple[int, bytes]], sender_ip: str, is_r expects_inline_qos=parse_bool(find_parameter(parameters, PID_EXPECTS_INLINE_QOS)) or False, unicast_address=endpoint_ip if endpoint_ip != "0.0.0.0" else sender_ip, unicast_port=endpoint_port, + multicast_locators=multicast_locators, ) endpoint_map = self.discovered_readers if is_reader else self.discovered_writers is_new = endpoint_guid not in endpoint_map endpoint_map[endpoint_guid] = endpoint + if not is_reader: + subscribed_topics = {reader.topic_name for reader in self.local_readers} + if endpoint.topic_name in subscribed_topics: + for multicast_address, multicast_port in endpoint.multicast_locators: + if multicast_port == self.ports.user_multicast: + self._join_user_multicast_group(multicast_address) if is_new: kind = "reader" if is_reader else "writer" log( @@ -836,11 +906,14 @@ def handle_user_packet(self, packet: bytes, sender_ip: str, sender_port: int) -> subscribed_topics = {reader.topic_name for reader in self.local_readers} if topic_name in subscribed_topics: writer = self.local_writers[0] - self._publish_value(writer, maybe_value, (sender_ip, sender_port)) - log( - f"[echo] responded with value={maybe_value} on '{writer.topic_name}' " - f"to {sender_ip}:{sender_port}" - ) + if self._publish_value(writer, maybe_value): + log(f"[echo] responded with value={maybe_value} on '{writer.topic_name}'") + else: + self._publish_value(writer, maybe_value, (sender_ip, sender_port)) + log( + f"[echo] responded with value={maybe_value} on '{writer.topic_name}' " + f"to {sender_ip}:{sender_port}" + ) def run(self) -> None: start_time = time.monotonic() @@ -884,6 +957,7 @@ def run(self) -> None: self.metatraffic_multicast_sock, self.metatraffic_unicast_sock, self.user_unicast_sock, + self.user_multicast_sock, ], [], [], @@ -892,7 +966,7 @@ def run(self) -> None: for sock in readable: packet, sender = sock.recvfrom(4096) sender_ip, sender_port = sender[0], sender[1] - if sock is self.user_unicast_sock: + if sock is self.user_unicast_sock or sock is self.user_multicast_sock: self.handle_user_packet(packet, sender_ip, sender_port) else: self.handle_metatraffic_packet(packet, sender_ip) @@ -906,6 +980,7 @@ def close(self) -> None: self.metatraffic_multicast_sock, self.metatraffic_unicast_sock, self.user_unicast_sock, + self.user_multicast_sock, ): try: sock.close() From 2f9437603dfdaa093adb6f93329cbb275e704c2e Mon Sep 17 00:00:00 2001 From: William Emfinger Date: Tue, 16 Jun 2026 13:30:26 -0500 Subject: [PATCH 08/32] Potential fix for pull request finding 'Empty except' Co-authored-by: Copilot Autofix powered by AI <223894421+github-code-quality[bot]@users.noreply.github.com> --- python/rtps_host.py | 2 ++ 1 file changed, 2 insertions(+) diff --git a/python/rtps_host.py b/python/rtps_host.py index 0aabadb38..5cd6d4beb 100644 --- a/python/rtps_host.py +++ b/python/rtps_host.py @@ -545,6 +545,8 @@ def _create_user_multicast_socket(self) -> socket.socket: try: sock.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEPORT, 1) except OSError: + # Some platforms expose SO_REUSEPORT but reject setting it; this is + # a best-effort optimization and is not required for correctness. pass sock.bind((self.args.bind_address, self.ports.user_multicast)) sock.setblocking(False) From 3b529fb5a59fe2e814ad278c608ba21e19b85fb2 Mon Sep 17 00:00:00 2001 From: William Emfinger Date: Tue, 16 Jun 2026 14:08:39 -0500 Subject: [PATCH 09/32] Potential fix for pull request finding Co-authored-by: Copilot Autofix powered by AI <175728472+Copilot@users.noreply.github.com> --- components/rtps/src/rtps.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/components/rtps/src/rtps.cpp b/components/rtps/src/rtps.cpp index 83bf00d8d..0cdc1e7af 100644 --- a/components/rtps/src/rtps.cpp +++ b/components/rtps/src/rtps.cpp @@ -1065,8 +1065,9 @@ std::vector RtpsParticipant::build_uint32_data_message(const WriterConf payload_writer.append_bytes(cdr); auto guid = writer_guid(writer_config.entity_index); - return build_message(guid_prefix_, {.value = kEntityIdUnknown}, guid.entity_id, 1, - payload_writer.take()) + static std::atomic sequence_number{1}; + return build_message(guid_prefix_, {.value = kEntityIdUnknown}, guid.entity_id, + sequence_number.fetch_add(1), payload_writer.take()) .serialize(); } From c4d76ef5eaaac68c6c4ec9d1bb86b72841dbb62b Mon Sep 17 00:00:00 2001 From: William Emfinger Date: Tue, 16 Jun 2026 14:09:07 -0500 Subject: [PATCH 10/32] Potential fix for pull request finding Co-authored-by: Copilot Autofix powered by AI <175728472+Copilot@users.noreply.github.com> --- components/rtps/src/rtps.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/components/rtps/src/rtps.cpp b/components/rtps/src/rtps.cpp index 0cdc1e7af..7b92f65f1 100644 --- a/components/rtps/src/rtps.cpp +++ b/components/rtps/src/rtps.cpp @@ -982,8 +982,9 @@ std::vector RtpsParticipant::build_spdp_announce_message() const { append_parameter_sentinel(parameters); auto payload = build_parameter_list_payload(parameters); - return build_message(guid_prefix_, {.value = kEntityIdUnknown}, {.value = kSpdpWriterEntityId}, 1, - payload) + static std::atomic sequence_number{1}; + return build_message(guid_prefix_, {.value = kEntityIdUnknown}, {.value = kSpdpWriterEntityId}, + sequence_number.fetch_add(1), payload) .serialize(); } From 78d6a3e9a09ce4882177cecc5fa26ad428df4684 Mon Sep 17 00:00:00 2001 From: William Emfinger Date: Tue, 16 Jun 2026 14:09:39 -0500 Subject: [PATCH 11/32] Potential fix for pull request finding Co-authored-by: Copilot Autofix powered by AI <175728472+Copilot@users.noreply.github.com> --- components/rtps/src/rtps.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/components/rtps/src/rtps.cpp b/components/rtps/src/rtps.cpp index 7b92f65f1..ebf0c6683 100644 --- a/components/rtps/src/rtps.cpp +++ b/components/rtps/src/rtps.cpp @@ -1015,9 +1015,10 @@ RtpsParticipant::build_sedp_publication_message(const WriterConfig &writer_confi append_parameter_sentinel(parameters); auto payload = build_parameter_list_payload(parameters); + static std::atomic sequence_number{1}; return build_message(guid_prefix_, {.value = kSedpPublicationsReaderEntityId}, {.value = kSedpPublicationsWriterEntityId}, - static_cast(writer_config.entity_index + 1), payload) + sequence_number.fetch_add(1), payload) .serialize(); } From 81f7bd69791d156e581dec7b85af70a95fde960d Mon Sep 17 00:00:00 2001 From: William Emfinger Date: Tue, 16 Jun 2026 14:10:00 -0500 Subject: [PATCH 12/32] Potential fix for pull request finding Co-authored-by: Copilot Autofix powered by AI <175728472+Copilot@users.noreply.github.com> --- components/rtps/src/rtps.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/components/rtps/src/rtps.cpp b/components/rtps/src/rtps.cpp index ebf0c6683..adc9bdd08 100644 --- a/components/rtps/src/rtps.cpp +++ b/components/rtps/src/rtps.cpp @@ -1048,9 +1048,10 @@ RtpsParticipant::build_sedp_subscription_message(const ReaderConfig &reader_conf append_parameter_sentinel(parameters); auto payload = build_parameter_list_payload(parameters); + static std::atomic sequence_number{1}; return build_message(guid_prefix_, {.value = kSedpSubscriptionsReaderEntityId}, {.value = kSedpSubscriptionsWriterEntityId}, - static_cast(reader_config.entity_index + 1), payload) + sequence_number.fetch_add(1), payload) .serialize(); } From 70da4b655915d18f4b365c0789186fb926e8517c Mon Sep 17 00:00:00 2001 From: William Emfinger Date: Tue, 16 Jun 2026 14:11:01 -0500 Subject: [PATCH 13/32] Potential fix for pull request finding Co-authored-by: Copilot Autofix powered by AI <175728472+Copilot@users.noreply.github.com> --- components/rtps/example/main/Kconfig.projbuild | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/components/rtps/example/main/Kconfig.projbuild b/components/rtps/example/main/Kconfig.projbuild index cdeb799f7..7d4a64dbc 100644 --- a/components/rtps/example/main/Kconfig.projbuild +++ b/components/rtps/example/main/Kconfig.projbuild @@ -30,7 +30,7 @@ menu "RTPS Example Configuration" config RTPS_EXAMPLE_DOMAIN_ID int "RTPS domain ID" - range 0 232 + range 0 231 default 0 help Both boards must use the same domain ID to discover each other. From ac317c6f17f5a3d3162be2c46eba328b86868a67 Mon Sep 17 00:00:00 2001 From: William Emfinger Date: Tue, 16 Jun 2026 14:11:41 -0500 Subject: [PATCH 14/32] Potential fix for pull request finding Co-authored-by: Copilot Autofix powered by AI <175728472+Copilot@users.noreply.github.com> --- components/rtps/src/rtps.cpp | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/components/rtps/src/rtps.cpp b/components/rtps/src/rtps.cpp index adc9bdd08..79f3379d8 100644 --- a/components/rtps/src/rtps.cpp +++ b/components/rtps/src/rtps.cpp @@ -1213,7 +1213,10 @@ bool RtpsParticipant::handle_metatraffic_message(std::vector &data, find_parameter(parameters, ParameterId::PID_DEFAULT_UNICAST_LOCATOR)) { if (auto maybe_locator = parse_locator(maybe_default_unicast_parameter->value)) { participant.ports.user_unicast = static_cast(maybe_locator->port); - participant.address = maybe_locator->address_string(); + const auto advertised_address = maybe_locator->address_string(); + if (advertised_address != "0.0.0.0") { + participant.address = advertised_address; + } } } if (auto maybe_default_multicast_parameter = From 20f4cd3b0802e20469ada50906561e87eccfd6c2 Mon Sep 17 00:00:00 2001 From: William Emfinger Date: Tue, 16 Jun 2026 14:20:38 -0500 Subject: [PATCH 15/32] actually fix issues --- components/rtps/include/rtps.hpp | 10 +++++++ components/rtps/src/rtps.cpp | 45 ++++++++++++++++++++++---------- 2 files changed, 41 insertions(+), 14 deletions(-) diff --git a/components/rtps/include/rtps.hpp b/components/rtps/include/rtps.hpp index e861cfab8..abe05a5aa 100644 --- a/components/rtps/include/rtps.hpp +++ b/components/rtps/include/rtps.hpp @@ -11,6 +11,7 @@ #include #include #include +#include #include #include "base_component.hpp" @@ -367,6 +368,10 @@ class RtpsParticipant : public BaseComponent { bool ensure_user_multicast_receivers_started(); std::vector build_user_send_configs(std::string_view topic_name, const WriterConfig &writer_config) const; + int64_t next_spdp_sequence_number() const; + int64_t next_sedp_publication_sequence_number() const; + int64_t next_sedp_subscription_sequence_number() const; + int64_t next_user_data_sequence_number(uint32_t entity_index) const; bool send_spdp_announce_now(); bool send_sedp_announcements_to(const ParticipantProxy &participant); bool send_discovery_now(); @@ -383,6 +388,11 @@ class RtpsParticipant : public BaseComponent { std::unique_ptr announce_task_; mutable std::mutex mutex_; + mutable std::mutex sequence_mutex_; + mutable std::atomic spdp_sequence_number_{1}; + mutable std::atomic sedp_publications_sequence_number_{1}; + mutable std::atomic sedp_subscriptions_sequence_number_{1}; + mutable std::unordered_map user_data_sequence_numbers_; std::vector writers_; std::vector readers_; std::vector discovered_participants_; diff --git a/components/rtps/src/rtps.cpp b/components/rtps/src/rtps.cpp index 79f3379d8..1a96e4b7a 100644 --- a/components/rtps/src/rtps.cpp +++ b/components/rtps/src/rtps.cpp @@ -477,11 +477,8 @@ std::optional find_parameter(std::span param std::vector find_parameters(std::span parameters, ParameterId id) { std::vector matches; - for (const auto ¶meter : parameters) { - if (parameter.id == id) { - matches.push_back(parameter); - } - } + std::copy_if(parameters.begin(), parameters.end(), std::back_inserter(matches), + [id](const auto ¶meter) { return parameter.id == id; }); return matches; } @@ -562,7 +559,9 @@ std::optional parse_locator(std::span @@ -982,9 +981,8 @@ std::vector RtpsParticipant::build_spdp_announce_message() const { append_parameter_sentinel(parameters); auto payload = build_parameter_list_payload(parameters); - static std::atomic sequence_number{1}; return build_message(guid_prefix_, {.value = kEntityIdUnknown}, {.value = kSpdpWriterEntityId}, - sequence_number.fetch_add(1), payload) + next_spdp_sequence_number(), payload) .serialize(); } @@ -1015,10 +1013,9 @@ RtpsParticipant::build_sedp_publication_message(const WriterConfig &writer_confi append_parameter_sentinel(parameters); auto payload = build_parameter_list_payload(parameters); - static std::atomic sequence_number{1}; return build_message(guid_prefix_, {.value = kSedpPublicationsReaderEntityId}, {.value = kSedpPublicationsWriterEntityId}, - sequence_number.fetch_add(1), payload) + next_sedp_publication_sequence_number(), payload) .serialize(); } @@ -1048,10 +1045,9 @@ RtpsParticipant::build_sedp_subscription_message(const ReaderConfig &reader_conf append_parameter_sentinel(parameters); auto payload = build_parameter_list_payload(parameters); - static std::atomic sequence_number{1}; return build_message(guid_prefix_, {.value = kSedpSubscriptionsReaderEntityId}, {.value = kSedpSubscriptionsWriterEntityId}, - sequence_number.fetch_add(1), payload) + next_sedp_subscription_sequence_number(), payload) .serialize(); } @@ -1068,9 +1064,9 @@ std::vector RtpsParticipant::build_uint32_data_message(const WriterConf payload_writer.append_bytes(cdr); auto guid = writer_guid(writer_config.entity_index); - static std::atomic sequence_number{1}; return build_message(guid_prefix_, {.value = kEntityIdUnknown}, guid.entity_id, - sequence_number.fetch_add(1), payload_writer.take()) + next_user_data_sequence_number(writer_config.entity_index), + payload_writer.take()) .serialize(); } @@ -1114,6 +1110,27 @@ bool RtpsParticipant::publish_uint32(std::string_view topic_name, uint32_t value return sent; } +int64_t RtpsParticipant::next_spdp_sequence_number() const { + return spdp_sequence_number_.fetch_add(1, std::memory_order_relaxed); +} + +int64_t RtpsParticipant::next_sedp_publication_sequence_number() const { + return sedp_publications_sequence_number_.fetch_add(1, std::memory_order_relaxed); +} + +int64_t RtpsParticipant::next_sedp_subscription_sequence_number() const { + return sedp_subscriptions_sequence_number_.fetch_add(1, std::memory_order_relaxed); +} + +int64_t RtpsParticipant::next_user_data_sequence_number(uint32_t entity_index) const { + std::lock_guard lock(sequence_mutex_); + auto iterator = user_data_sequence_numbers_.try_emplace(entity_index, 1).first; + auto &sequence_number = iterator->second; + int64_t current = sequence_number; + sequence_number++; + return current; +} + std::vector RtpsParticipant::serialize_uint32_cdr(uint32_t value) { espp::CdrWriter writer({ .encapsulation = espp::CdrEncapsulation::CDR_LE, From 1390dbe71e4b086e73fd35f2dc09f803fd73e6c9 Mon Sep 17 00:00:00 2001 From: William Emfinger Date: Tue, 16 Jun 2026 17:07:05 -0500 Subject: [PATCH 16/32] fix issues --- components/cdr/include/cdr.hpp | 15 ++++- components/rtps/include/rtps.hpp | 9 ++- components/rtps/src/rtps.cpp | 112 ++++++++++++++++++++++++------- 3 files changed, 106 insertions(+), 30 deletions(-) diff --git a/components/cdr/include/cdr.hpp b/components/cdr/include/cdr.hpp index c8f47a216..f595230fd 100644 --- a/components/cdr/include/cdr.hpp +++ b/components/cdr/include/cdr.hpp @@ -75,6 +75,11 @@ class CdrWriter { /// @brief Create a writer configured for a headerless/body-only CDR payload. /// @param encapsulation Endianness/encapsulation rules to use for the body payload. /// @return A ready-to-use writer with no encapsulation header in its output. + /// @note CDR alignment is measured from the start of the buffer. Because a body-only writer has + /// no 4-byte encapsulation header, 8-byte-aligned members (e.g. int64/double) land at different + /// offsets than in an encapsulated writer. A body produced here and later wrapped with + /// encapsulate() is therefore not byte-compatible with a directly-encapsulated buffer when it + /// contains 8-byte-aligned types. Current RTPS usage only emits <= 4-byte-aligned types. [[nodiscard]] static CdrWriter make_body_writer(CdrEncapsulation encapsulation = CdrEncapsulation::CDR_LE) { return CdrWriter(body_config(encapsulation)); @@ -424,6 +429,12 @@ class CdrReader { span = span.first(span.size() - 1); } text.assign(reinterpret_cast(span.data()), span.size()); + // Re-align for any following element. CDR only inserts padding before an aligned element, so a + // trailing string at the end of the buffer may legitimately have no padding bytes; only align + // when there are bytes left to consume so a valid final string is not rejected. + if (remaining() == 0) { + return true; + } return align(4); } @@ -500,7 +511,9 @@ class CdrReader { return false; } values.clear(); - values.reserve(length); + // Cap the reservation against the bytes actually available so a malformed length cannot trigger + // a huge allocation. The element loop below still validates each read. + values.reserve(std::min(length, remaining() / sizeof(T))); for (uint32_t i = 0; i < length; i++) { T value{}; if (!read(value)) { diff --git a/components/rtps/include/rtps.hpp b/components/rtps/include/rtps.hpp index abe05a5aa..865c8a395 100644 --- a/components/rtps/include/rtps.hpp +++ b/components/rtps/include/rtps.hpp @@ -283,12 +283,12 @@ class RtpsParticipant : public BaseComponent { std::vector discovered_readers() const; /// @brief Access the registered local writer configurations. - /// @return A const reference to the local writer list. - const std::vector &writers() const; + /// @return A snapshot copy of the local writer list. + std::vector writers() const; /// @brief Access the registered local reader configurations. - /// @return A const reference to the local reader list. - const std::vector &readers() const; + /// @return A snapshot copy of the local reader list. + std::vector readers() const; /// @brief Compute the standard RTPS UDP port mapping for this participant. /// @return The derived metatraffic and user-data ports for the configured domain and participant @@ -375,7 +375,6 @@ class RtpsParticipant : public BaseComponent { bool send_spdp_announce_now(); bool send_sedp_announcements_to(const ParticipantProxy &participant); bool send_discovery_now(); - ParticipantProxy make_local_participant_proxy() const; Config config_; GuidPrefix guid_prefix_{}; diff --git a/components/rtps/src/rtps.cpp b/components/rtps/src/rtps.cpp index 1a96e4b7a..b1e6ad7a6 100644 --- a/components/rtps/src/rtps.cpp +++ b/components/rtps/src/rtps.cpp @@ -176,6 +176,20 @@ class ByteReader { return true; } + bool read_u16_be(uint16_t &value) { + if (remaining() < 2) { + return false; + } + value = static_cast(static_cast(data_[offset_]) << 8) | + static_cast(data_[offset_ + 1]); + offset_ += 2; + return true; + } + + bool read_u16(uint16_t &value, bool little_endian) { + return little_endian ? read_u16_le(value) : read_u16_be(value); + } + bool read_u32_le(uint32_t &value) { if (remaining() < 4) { return false; @@ -209,13 +223,27 @@ class ByteReader { return true; } - bool read_sequence_number_le(int64_t &value) { - int32_t high = 0; + bool read_sequence_number(int64_t &value, bool little_endian) { + uint32_t high = 0; uint32_t low = 0; - if (!read_i32_le(high) || !read_u32_le(low)) { + if (little_endian) { + if (!read_u32_le(high) || !read_u32_le(low)) { + return false; + } + } else { + if (!read_u32_be(high) || !read_u32_be(low)) { + return false; + } + } + value = (static_cast(static_cast(high)) << 32) | low; + return true; + } + + bool skip(size_t length) { + if (remaining() < length) { return false; } - value = (static_cast(high) << 32) | low; + offset_ += length; return true; } @@ -378,22 +406,27 @@ void append_parameter_locator(ByteWriter &writer, ParameterId id, } void append_parameter_string_cdr(ByteWriter &writer, ParameterId id, std::string_view text) { - uint16_t raw_length = static_cast(4 + text.size() + 1); - append_parameter_header(writer, id, raw_length); auto cdr_writer = espp::CdrWriter::make_body_writer(espp::CdrEncapsulation::CDR_LE); cdr_writer.write_string(text); - writer.append_bytes(cdr_writer.payload()); + auto cdr_payload = cdr_writer.payload(); + // The RTPS PL_CDR encoding requires parameterLength to be a multiple of 4 so the next + // parameter starts 4-byte aligned. write_string() already trailing-aligns the body to 4, so the + // payload length is the padded length we must declare. + append_parameter_header(writer, id, static_cast(cdr_payload.size())); + writer.append_bytes(cdr_payload); } void append_parameter_octet_sequence(ByteWriter &writer, ParameterId id, std::span bytes) { - uint16_t raw_length = static_cast(4 + bytes.size()); - append_parameter_header(writer, id, raw_length); auto cdr_writer = espp::CdrWriter::make_body_writer(espp::CdrEncapsulation::CDR_LE); cdr_writer.write(static_cast(bytes.size())); cdr_writer.write_bytes(bytes); cdr_writer.align(4); - writer.append_bytes(cdr_writer.payload()); + auto cdr_payload = cdr_writer.payload(); + // parameterLength must be a multiple of 4 (see append_parameter_string_cdr); the align(4) above + // padded the payload to that length, so declare the padded length. + append_parameter_header(writer, id, static_cast(cdr_payload.size())); + writer.append_bytes(cdr_payload); } void append_parameter_reliability(ByteWriter &writer, @@ -437,6 +470,11 @@ void append_parameter_sentinel(ByteWriter &writer) { std::vector parse_parameter_list(std::span payload) { std::vector parameters; espp::CdrReader cdr_reader(payload); + // Limitation: only little-endian parameter lists (PL_CDR_LE) are decoded. The parameter value + // parsers below (parse_u32_le, parse_locator, parse_guid, ...) assume little-endian contents, so + // a big-endian (PL_CDR_BE) list is intentionally rejected rather than misparsed. In practice DDS + // and ROS 2 implementations emit PL_CDR_LE for SPDP/SEDP discovery, so this is a discovery-only + // gap. if (!cdr_reader.valid() || cdr_reader.encapsulation() != espp::CdrEncapsulation::PL_CDR_LE) { return parameters; } @@ -600,6 +638,25 @@ bool is_same_guid_prefix(const espp::RtpsParticipant::Guid &guid, return guid.prefix == prefix; } +// Skip an inline-QoS parameter list (a raw ParameterList without an encapsulation header) up to and +// including its PID_SENTINEL terminator. Returns false if the list is malformed/truncated. +bool skip_inline_qos(ByteReader &reader, bool little_endian) { + while (reader.remaining() >= 4) { + uint16_t pid = 0; + uint16_t length = 0; + if (!reader.read_u16(pid, little_endian) || !reader.read_u16(length, little_endian)) { + return false; + } + if (pid == static_cast(ParameterId::PID_SENTINEL)) { + return true; + } + if (!reader.skip(length)) { + return false; + } + } + return false; +} + DataSubmessageView parse_data_submessage(const espp::RtpsParticipant::Submessage &submessage, bool &ok) { DataSubmessageView view; @@ -609,21 +666,35 @@ DataSubmessageView parse_data_submessage(const espp::RtpsParticipant::Submessage return view; } + const bool little_endian = (submessage.flags & kSubmessageFlagLittleEndian) != 0; ByteReader reader(std::span{submessage.payload.data(), submessage.payload.size()}); uint16_t extra_flags = 0; uint16_t octets_to_inline_qos = 0; - if (!reader.read_u16_le(extra_flags) || !reader.read_u16_le(octets_to_inline_qos) || + if (!reader.read_u16(extra_flags, little_endian) || + !reader.read_u16(octets_to_inline_qos, little_endian) || !reader.read_bytes( std::span{view.reader_id.value.data(), view.reader_id.value.size()}) || !reader.read_bytes( std::span{view.writer_id.value.data(), view.writer_id.value.size()}) || - !reader.read_sequence_number_le(view.writer_sn)) { + !reader.read_sequence_number(view.writer_sn, little_endian)) { return view; } view.inline_qos_present = (submessage.flags & kSubmessageFlagInlineQos) != 0; view.data_present = true; - if (view.inline_qos_present || octets_to_inline_qos != kDataSubmessageOctetsToInlineQos) { + + // octetsToInlineQos counts from the byte after the octetsToInlineQos field to the start of the + // inline QoS (or the serialized payload when no inline QoS is present). We have already consumed + // the standard 16-byte readerId+writerId+writerSN block; honor any additional header octets a + // sender may have included instead of assuming the fixed layout. + if (octets_to_inline_qos < kDataSubmessageOctetsToInlineQos || + !reader.skip(octets_to_inline_qos - kDataSubmessageOctetsToInlineQos)) { + return view; + } + + // When inline QoS is present, skip past the inline QoS parameter list to reach the serialized + // payload rather than dropping the sample. + if (view.inline_qos_present && !skip_inline_qos(reader, little_endian)) { return view; } @@ -919,11 +990,13 @@ std::vector RtpsParticipant::discovered_readers( return discovered_readers_; } -const std::vector &RtpsParticipant::writers() const { +std::vector RtpsParticipant::writers() const { + std::lock_guard lock(mutex_); return writers_; } -const std::vector &RtpsParticipant::readers() const { +std::vector RtpsParticipant::readers() const { + std::lock_guard lock(mutex_); return readers_; } @@ -1616,13 +1689,4 @@ bool RtpsParticipant::send_discovery_now() { }); } -RtpsParticipant::ParticipantProxy RtpsParticipant::make_local_participant_proxy() const { - return {.participant_guid = participant_guid(), - .guid_prefix = guid_prefix_, - .name = config_.node_name, - .enclave = config_.enclave, - .address = config_.advertised_address, - .ports = ports(), - .builtin_endpoints = kBuiltinEndpointSet}; -} } // namespace espp From aa4cfd210e8070374b82422bca84c29509ba11a7 Mon Sep 17 00:00:00 2001 From: William Emfinger Date: Tue, 16 Jun 2026 20:09:53 -0500 Subject: [PATCH 17/32] separate socket and rtps log levels --- components/rtps/example/main/rtps_example.cpp | 3 +++ components/rtps/include/rtps.hpp | 5 +++++ components/rtps/src/rtps.cpp | 9 +++++---- 3 files changed, 13 insertions(+), 4 deletions(-) diff --git a/components/rtps/example/main/rtps_example.cpp b/components/rtps/example/main/rtps_example.cpp index 6cea5bdc4..3abe97f85 100644 --- a/components/rtps/example/main/rtps_example.cpp +++ b/components/rtps/example/main/rtps_example.cpp @@ -132,6 +132,9 @@ extern "C" void app_main(void) { endpoint.topic_name, endpoint.type_name); }, .log_level = espp::Logger::Verbosity::INFO, + // Keep the underlying UDP sockets quieter than the participant so routine socket activity + // does not clutter the logs. Raise this to debug transport issues independently. + .socket_log_level = espp::Logger::Verbosity::WARN, }); #if CONFIG_RTPS_EXAMPLE_ROLE_INITIATOR diff --git a/components/rtps/include/rtps.hpp b/components/rtps/include/rtps.hpp index 865c8a395..57356ebe9 100644 --- a/components/rtps/include/rtps.hpp +++ b/components/rtps/include/rtps.hpp @@ -238,6 +238,11 @@ class RtpsParticipant : public BaseComponent { nullptr}; ///< Callback invoked when a remote endpoint is first discovered. espp::Logger::Verbosity log_level{ espp::Logger::Verbosity::INFO}; ///< Participant log verbosity. + espp::Logger::Verbosity socket_log_level{ + espp::Logger::Verbosity::WARN}; ///< Log verbosity for the participant's underlying UDP + ///< sockets. Defaults to WARN so routine socket activity + ///< does not clutter the logs; raise it to debug transport + ///< issues independently of the participant log level. }; /// @brief Construct an RTPS participant. diff --git a/components/rtps/src/rtps.cpp b/components/rtps/src/rtps.cpp index b1e6ad7a6..07be446e5 100644 --- a/components/rtps/src/rtps.cpp +++ b/components/rtps/src/rtps.cpp @@ -845,11 +845,11 @@ bool RtpsParticipant::start() { auto port_mapping = ports(); metatraffic_multicast_receiver_ = - std::make_unique(UdpSocket::Config{.log_level = get_log_level()}); + std::make_unique(UdpSocket::Config{.log_level = config_.socket_log_level}); metatraffic_unicast_receiver_ = - std::make_unique(UdpSocket::Config{.log_level = get_log_level()}); + std::make_unique(UdpSocket::Config{.log_level = config_.socket_log_level}); user_unicast_receiver_ = - std::make_unique(UdpSocket::Config{.log_level = get_log_level()}); + std::make_unique(UdpSocket::Config{.log_level = config_.socket_log_level}); auto multicast_task_config = config_.receive_task_config; multicast_task_config.name = config_.receive_task_config.name + "_spdp_mc"; @@ -1532,7 +1532,8 @@ bool RtpsParticipant::ensure_user_multicast_receivers_started() { continue; } - auto socket = std::make_unique(UdpSocket::Config{.log_level = get_log_level()}); + auto socket = + std::make_unique(UdpSocket::Config{.log_level = config_.socket_log_level}); auto task_config = config_.receive_task_config; task_config.name = fmt::format("{}_user_mc_{}", config_.receive_task_config.name, user_multicast_receivers_.size()); From 9e16e288a7bf752bc36288cbe31acf0121b2dadd Mon Sep 17 00:00:00 2001 From: William Emfinger Date: Wed, 17 Jun 2026 13:10:47 -0500 Subject: [PATCH 18/32] improve robustness and api correctness, esp. w.r.t. string operations --- components/cdr/include/cdr.hpp | 20 +++++++++++++----- components/rtps/include/rtps.hpp | 2 +- components/rtps/src/rtps.cpp | 35 ++++++++++++++++++++------------ 3 files changed, 38 insertions(+), 19 deletions(-) diff --git a/components/cdr/include/cdr.hpp b/components/cdr/include/cdr.hpp index f595230fd..1dff9d53d 100644 --- a/components/cdr/include/cdr.hpp +++ b/components/cdr/include/cdr.hpp @@ -425,9 +425,13 @@ class CdrReader { } auto span = data_.subspan(offset_, length); offset_ += length; - if (span.back() == 0) { - span = span.first(span.size() - 1); + // CDR strings are length-prefixed and null-terminated; a missing terminator is a malformed + // payload, so reject it rather than silently accepting the bytes. + if (span.back() != 0) { + valid_ = false; + return false; } + span = span.first(span.size() - 1); text.assign(reinterpret_cast(span.data()), span.size()); // Re-align for any following element. CDR only inserts padding before an aligned element, so a // trailing string at the end of the buffer may legitimately have no padding bytes; only align @@ -510,10 +514,16 @@ class CdrReader { if (!read(length)) { return false; } + // Bound the declared length against the bytes actually available before reserving so a + // malformed/malicious payload cannot request an enormous allocation (or OOM) on a + // memory-constrained target. Each element occupies at least sizeof(T) bytes, so a length larger + // than remaining() / sizeof(T) cannot possibly be satisfied; reject it up front. + if (length > remaining() / sizeof(T)) { + valid_ = false; + return false; + } values.clear(); - // Cap the reservation against the bytes actually available so a malformed length cannot trigger - // a huge allocation. The element loop below still validates each read. - values.reserve(std::min(length, remaining() / sizeof(T))); + values.reserve(length); for (uint32_t i = 0; i < length; i++) { T value{}; if (!read(value)) { diff --git a/components/rtps/include/rtps.hpp b/components/rtps/include/rtps.hpp index 57356ebe9..841e6d8a3 100644 --- a/components/rtps/include/rtps.hpp +++ b/components/rtps/include/rtps.hpp @@ -370,7 +370,7 @@ class RtpsParticipant : public BaseComponent { bool handle_metatraffic_message(std::vector &data, const Socket::Info &sender); bool handle_user_message(std::vector &data, const Socket::Info &sender); - bool ensure_user_multicast_receivers_started(); + bool ensure_user_multicast_receivers_started(const std::string &extra_group = {}); std::vector build_user_send_configs(std::string_view topic_name, const WriterConfig &writer_config) const; int64_t next_spdp_sequence_number() const; diff --git a/components/rtps/src/rtps.cpp b/components/rtps/src/rtps.cpp index 07be446e5..336bb063d 100644 --- a/components/rtps/src/rtps.cpp +++ b/components/rtps/src/rtps.cpp @@ -626,6 +626,10 @@ std::string extract_enclave(std::span user_data_bytes) { if (end == std::string::npos) { end = text.size(); } + // Normalize an empty enclave (e.g. "enclave=;") to the default "/" rather than returning "". + if (end == position) { + return "/"; + } return text.substr(position, end - position); } @@ -963,15 +967,15 @@ bool RtpsParticipant::add_writer(const WriterConfig &writer_config) { } bool RtpsParticipant::add_reader(const ReaderConfig &reader_config) { - { - std::lock_guard lock(mutex_); - readers_.push_back(reader_config); - } + // Bring up the reader's multicast receiver (if any) before persisting the reader, so a failure + // does not leave the participant with a registered reader that has no working receiver. if (started_.load() && !reader_config.multicast_group.empty() && - !ensure_user_multicast_receivers_started()) { + !ensure_user_multicast_receivers_started(reader_config.multicast_group)) { logger_.error("Failed to start multicast receiver for topic '{}'", reader_config.topic_name); return false; } + std::lock_guard lock(mutex_); + readers_.push_back(reader_config); return true; } @@ -1503,23 +1507,28 @@ bool RtpsParticipant::handle_user_message(std::vector &data, const Sock return false; } -bool RtpsParticipant::ensure_user_multicast_receivers_started() { +bool RtpsParticipant::ensure_user_multicast_receivers_started(const std::string &extra_group) { if (!started_.load()) { return true; } std::vector desired_groups; - if (config_.use_multicast_for_user_data && !config_.user_multicast_group.empty()) { - desired_groups.push_back(config_.user_multicast_group); + auto add_group = [&desired_groups](const std::string &group) { + if (!group.empty() && + std::find(desired_groups.begin(), desired_groups.end(), group) == desired_groups.end()) { + desired_groups.push_back(group); + } + }; + if (config_.use_multicast_for_user_data) { + add_group(config_.user_multicast_group); } + // Include the group of a reader being added before it is persisted in readers_, so the receiver + // can be brought up (and any failure surfaced) without leaving the reader half-registered. + add_group(extra_group); { std::lock_guard lock(mutex_); for (const auto &reader_config : readers_) { - if (!reader_config.multicast_group.empty() && - std::find(desired_groups.begin(), desired_groups.end(), reader_config.multicast_group) == - desired_groups.end()) { - desired_groups.push_back(reader_config.multicast_group); - } + add_group(reader_config.multicast_group); } } From 1e23f185b4429168bfe3ef8773e99977114ce1a6 Mon Sep 17 00:00:00 2001 From: William Emfinger Date: Wed, 17 Jun 2026 13:26:58 -0500 Subject: [PATCH 19/32] fix race --- components/rtps/include/rtps.hpp | 2 ++ components/rtps/src/rtps.cpp | 18 ++++++++++++++---- 2 files changed, 16 insertions(+), 4 deletions(-) diff --git a/components/rtps/include/rtps.hpp b/components/rtps/include/rtps.hpp index 841e6d8a3..45500ef70 100644 --- a/components/rtps/include/rtps.hpp +++ b/components/rtps/include/rtps.hpp @@ -392,6 +392,8 @@ class RtpsParticipant : public BaseComponent { std::unique_ptr announce_task_; mutable std::mutex mutex_; + mutable std::mutex receivers_mutex_; ///< Guards user_multicast_receivers_ against concurrent + ///< add_reader()/stop() access. mutable std::mutex sequence_mutex_; mutable std::atomic spdp_sequence_number_{1}; mutable std::atomic sedp_publications_sequence_number_{1}; diff --git a/components/rtps/src/rtps.cpp b/components/rtps/src/rtps.cpp index 336bb063d..d8e6f88db 100644 --- a/components/rtps/src/rtps.cpp +++ b/components/rtps/src/rtps.cpp @@ -813,6 +813,10 @@ RtpsParticipant::Message::parse(std::span data) { Submessage submessage; uint8_t kind = 0; uint16_t length = 0; + // Limitation: submessageLength is read little-endian regardless of the submessage E-flag (bit 0 + // of flags). Big-endian submessages are not supported; in practice DDS/ROS 2 peers emit + // little-endian framing. Endianness of the DATA submessage body itself is honored separately in + // parse_data_submessage(). if (!reader.read_u8(kind) || !reader.read_u8(submessage.flags) || !reader.read_u16_le(length)) { return std::nullopt; } @@ -946,12 +950,15 @@ void RtpsParticipant::stop() { metatraffic_unicast_receiver_->stop_receiving(); metatraffic_unicast_receiver_.reset(); } - for (auto &receiver : user_multicast_receivers_) { - if (receiver.socket) { - receiver.socket->stop_receiving(); + { + std::lock_guard receivers_lock(receivers_mutex_); + for (auto &receiver : user_multicast_receivers_) { + if (receiver.socket) { + receiver.socket->stop_receiving(); + } } + user_multicast_receivers_.clear(); } - user_multicast_receivers_.clear(); if (user_unicast_receiver_) { user_unicast_receiver_->stop_receiving(); user_unicast_receiver_.reset(); @@ -1533,6 +1540,9 @@ bool RtpsParticipant::ensure_user_multicast_receivers_started(const std::string } auto port_mapping = ports(); + // receivers_mutex_ (not mutex_) guards user_multicast_receivers_; desired_groups was built above + // under mutex_, which has already been released, so the two locks are never held nested. + std::lock_guard receivers_lock(receivers_mutex_); for (const auto &group : desired_groups) { auto existing = std::find_if(user_multicast_receivers_.begin(), user_multicast_receivers_.end(), From 3943cb41ebdbb55d0755e0e3e350b9013b134187 Mon Sep 17 00:00:00 2001 From: William Emfinger Date: Wed, 17 Jun 2026 16:57:48 -0500 Subject: [PATCH 20/32] improve spec compliance --- components/rtps/src/rtps.cpp | 38 ++++++++++++++++++++---------------- 1 file changed, 21 insertions(+), 17 deletions(-) diff --git a/components/rtps/src/rtps.cpp b/components/rtps/src/rtps.cpp index d8e6f88db..319a348f6 100644 --- a/components/rtps/src/rtps.cpp +++ b/components/rtps/src/rtps.cpp @@ -62,6 +62,9 @@ constexpr int32_t kDefaultLeaseDurationSeconds = 20; constexpr uint32_t kDefaultLeaseDurationNanoseconds = 0; constexpr int32_t kDefaultMaxBlockingSeconds = 0; constexpr uint32_t kDefaultMaxBlockingNanoseconds = 100000000; +// PID_TYPE_MAX_SIZE_SERIALIZED carries the max CDR-serialized size of the type *including* the +// 4-byte encapsulation header (matching FastDDS: getMaxCdrSerializedSize() + 4). A UInt32 body is +// 4 bytes, so the spec-exact advertised value is 4 + 4 = 8. constexpr uint32_t kUInt32SerializedSize = 8; enum class ParameterId : uint16_t { @@ -113,11 +116,6 @@ class ByteWriter { data_.push_back(static_cast((value >> 8) & 0xff)); } - void append_u16_be(uint16_t value) { - data_.push_back(static_cast((value >> 8) & 0xff)); - data_.push_back(static_cast(value & 0xff)); - } - void append_u32_le(uint32_t value) { for (int i = 0; i < 4; i++) { data_.push_back(static_cast((value >> (8 * i)) & 0xff)); @@ -126,12 +124,6 @@ class ByteWriter { void append_i32_le(int32_t value) { append_u32_le(static_cast(value)); } - void append_u32_be(uint32_t value) { - for (int i = 3; i >= 0; i--) { - data_.push_back(static_cast((value >> (8 * i)) & 0xff)); - } - } - void append_sequence_number_le(int64_t value) { auto high = static_cast(value >> 32); auto low = static_cast(value & 0xffffffffu); @@ -390,18 +382,28 @@ void append_parameter_bool(ByteWriter &writer, ParameterId id, bool value) { writer.append_u8(0); } +// RTPS Duration_t/Time_t use the NTP representation {int32 seconds, uint32 fraction} where the +// fraction is in units of 1/2^32 of a second (see DDSI-RTPS; OpenDDS RtpsCore.idl references RFC +// 1305). Convert a nanosecond count to that fraction so durations are encoded spec-exactly. +constexpr uint32_t ntp_fraction_from_nanoseconds(uint32_t nanoseconds) { + return static_cast((static_cast(nanoseconds) << 32) / 1000000000ULL); +} + void append_parameter_duration(ByteWriter &writer, ParameterId id, int32_t seconds, uint32_t nanoseconds) { append_parameter_header(writer, id, 8); writer.append_i32_le(seconds); - writer.append_u32_le(nanoseconds); + writer.append_u32_le(ntp_fraction_from_nanoseconds(nanoseconds)); } void append_parameter_locator(ByteWriter &writer, ParameterId id, const espp::RtpsParticipant::Locator &locator) { append_parameter_header(writer, id, 24); - writer.append_u32_be(static_cast(locator.kind)); - writer.append_u32_be(locator.port); + // Locator_t.kind and .port are CDR long/unsigned long encoded in the parameter list endianness + // (little-endian for PL_CDR_LE); only the 16-byte address is a raw per-byte (network-order) + // field. + writer.append_u32_le(static_cast(locator.kind)); + writer.append_u32_le(locator.port); writer.append_bytes(locator.address); } @@ -436,7 +438,7 @@ void append_parameter_reliability(ByteWriter &writer, ? kReliabilityReliable : kReliabilityBestEffort); writer.append_i32_le(kDefaultMaxBlockingSeconds); - writer.append_u32_le(kDefaultMaxBlockingNanoseconds); + writer.append_u32_le(ntp_fraction_from_nanoseconds(kDefaultMaxBlockingNanoseconds)); } void append_parameter_durability(ByteWriter &writer) { @@ -448,7 +450,7 @@ void append_parameter_liveliness(ByteWriter &writer) { append_parameter_header(writer, ParameterId::PID_LIVELINESS, 12); writer.append_u32_le(kLivelinessAutomatic); writer.append_i32_le(kDefaultLeaseDurationSeconds); - writer.append_u32_le(kDefaultLeaseDurationNanoseconds); + writer.append_u32_le(ntp_fraction_from_nanoseconds(kDefaultLeaseDurationNanoseconds)); } void append_parameter_history(ByteWriter &writer) { @@ -587,7 +589,9 @@ std::optional parse_locator(std::span{locator.address.data(), locator.address.size()})) { return std::nullopt; } From 6efb044e33c5c5f65785aad95907f3202fcd2732 Mon Sep 17 00:00:00 2001 From: William Emfinger Date: Wed, 17 Jun 2026 21:31:27 -0500 Subject: [PATCH 21/32] wip improved api --- components/rtps/README.md | 21 ++-- components/rtps/example/main/rtps_example.cpp | 28 +++-- components/rtps/include/rtps.hpp | 28 +++-- components/rtps/src/rtps.cpp | 112 +++++++----------- 4 files changed, 88 insertions(+), 101 deletions(-) diff --git a/components/rtps/README.md b/components/rtps/README.md index c07cd98a0..4fe74aef8 100644 --- a/components/rtps/README.md +++ b/components/rtps/README.md @@ -30,9 +30,12 @@ RTPS separates *metatraffic* from *user traffic*. - **Metatraffic** carries discovery and endpoint metadata. In this component, that means SPDP participant announcements plus SEDP publication and subscription announcements. -- **User traffic** carries application samples. The current ESPP scaffold has a - temporary best-effort `UInt32` user-data path while the standards-based - ROS 2 data plane is still being completed. +- **User traffic** carries application samples. Samples are sent as standard + RTPS `DATA` submessages whose `serializedPayload` is the raw CDR-encapsulated + sample (no ESPP-specific framing); the topic is identified by the writer GUID + resolved through SEDP discovery. The `publish()` / `on_sample` API works with + any CDR-encoded type. Reliable delivery (`HEARTBEAT`/`ACKNACK`) is not yet + implemented, so user traffic is currently best-effort. The current `RtpsParticipant` implementation opens three UDP sockets when `start()` is called: @@ -85,9 +88,9 @@ against every stack". | Peer implementation | Expected compatibility | Notes | | --- | --- | --- | -| ESPP `rtps` component / `python/rtps_host.py` | **Yes** for current scaffold | Intended smoke-test path for SPDP, SEDP, and the temporary `UInt32` `ESPPDATA` user-data payload. | -| Generic DDSI-RTPS 2.3 implementations | **Partial** | SPDP and SEDP messages are standards-shaped, but only the discovery slice is implemented today. | -| ROS 2 nodes backed by Fast DDS | **Partial / discovery-targeted** | The current discovery messages include ROS 2-relevant participant user data such as `enclave=...;`, but standards-based ROS 2 topic data exchange is not finished yet. | +| ESPP `rtps` component / `python/rtps_host.py` | **Yes** for current scaffold | Intended smoke-test path for SPDP, SEDP, and the standard CDR-over-RTPS best-effort user-data path. | +| Generic DDSI-RTPS 2.3 implementations | **Partial** | SPDP, SEDP, and best-effort `DATA` samples are standards-shaped; reliable delivery is not implemented today. | +| ROS 2 nodes backed by Fast DDS | **Partial / discovery-targeted** | Discovery includes ROS 2-relevant participant user data such as `enclave=...;`, and user samples are standard CDR-over-RTPS. Full ROS 2 topic interop additionally needs ROS 2 topic/type name mangling (`rt/...`, `std_msgs::msg::dds_::UInt32_`). | | ROS 2 nodes backed by Cyclone DDS or other DDS vendors | **Partial / unverified** | Expected to be limited to the minimal discovery subset if the peer accepts the currently emitted parameter set; not validated yet. | | Reliable DDS/RTPS endpoints | **No** | `HEARTBEAT`, `ACKNACK`, retransmission windows, and other reliable state-machine pieces are not implemented. | @@ -100,12 +103,12 @@ against every stack". | SPDP participant announce send/receive | **Implemented** | Multicast announce plus participant cache updates. | | SEDP publication / subscription announce send/receive | **Implemented** | Local endpoints are announced and remote endpoints are cached. | | Participant / endpoint discovery callbacks | **Implemented** | Exposed through `on_participant_discovered` and `on_endpoint_discovered`. | -| Temporary `UInt32` user-data path | **Implemented** | Uses the current ESPP-specific `ESPPDATA` payload, not a standards-based DDS sample representation. | +| Standard CDR-over-RTPS user-data path | **Implemented** | `DATA` `serializedPayload` is the raw CDR sample; the `publish()` / `on_sample` API carries any CDR-encoded type, routed by writer GUID via SEDP. | | Best-effort user-data multicast transport | **Implemented** | Supports shared participant-level multicast or endpoint-specific multicast locators advertised in SEDP; local readers only join the multicast groups configured for their topics. | | QoS fields emitted in discovery | **Partial** | Reliability, durability, liveliness, and history parameters are advertised in SEDP. | | QoS matching / policy enforcement | **Not implemented** | Remote QoS is parsed, but full writer/reader matching logic is still missing. | -| Standards-based DDS user-data serialization | **Not implemented** | The current data path is a temporary ESPP scaffold for `std_msgs/msg/UInt32`. | -| Inline QoS handling | **Not implemented** | Discovery and user-data handling assume no inline QoS. | +| ROS 2 topic/type name mangling | **Not implemented** | Topic/type names are emitted verbatim; ROS 2 interop needs `rt/...` topic and `std_msgs::msg::dds_::UInt32_` type mangling. | +| Inline QoS handling | **Partial** | Inline QoS is skipped on receive to reach the payload; it is not emitted or interpreted. | | Reliable RTPS (`HEARTBEAT`, `ACKNACK`, resend`) | **Not implemented** | Reliable delivery is not interoperable yet. | | Full ROS 2 topic interoperability | **Not implemented** | Discovery is the current milestone; ROS 2-compatible data writers/readers are still pending. | diff --git a/components/rtps/example/main/rtps_example.cpp b/components/rtps/example/main/rtps_example.cpp index 3abe97f85..97e03ce8a 100644 --- a/components/rtps/example/main/rtps_example.cpp +++ b/components/rtps/example/main/rtps_example.cpp @@ -151,10 +151,14 @@ extern "C" void app_main(void) { .reliability = espp::RtpsParticipant::ReliabilityKind::BEST_EFFORT, .multicast_group = response_multicast_group, .entity_index = 0, - .on_uint32_sample = - [&logger, &response_count, &last_sent_request](uint32_t value) { + .on_sample = + [&logger, &response_count, &last_sent_request](std::span cdr) { + auto value = espp::RtpsParticipant::deserialize_uint32_cdr(cdr); + if (!value) { + return; + } response_count++; - logger.info("Received response {} (expected {})", value, last_sent_request.load()); + logger.info("Received response {} (expected {})", *value, last_sent_request.load()); }, }); #else @@ -172,12 +176,18 @@ extern "C" void app_main(void) { .reliability = espp::RtpsParticipant::ReliabilityKind::BEST_EFFORT, .multicast_group = request_multicast_group, .entity_index = 0, - .on_uint32_sample = - [&logger, &request_count, &response_topic, &participant_ptr](uint32_t value) { + .on_sample = + [&logger, &request_count, &response_topic, + &participant_ptr](std::span cdr) { + auto value = espp::RtpsParticipant::deserialize_uint32_cdr(cdr); + if (!value) { + return; + } request_count++; - logger.info("Received request {}, sending response", value); - if (!participant_ptr->publish_uint32(response_topic, value)) { - logger.warn("Failed to publish response {}", value); + logger.info("Received request {}, sending response", *value); + if (!participant_ptr->publish(response_topic, + espp::RtpsParticipant::serialize_uint32_cdr(*value))) { + logger.warn("Failed to publish response {}", *value); } }, }); @@ -230,7 +240,7 @@ extern "C" void app_main(void) { auto value = next_request_value.fetch_add(1); last_sent_request = value; - if (participant.publish_uint32(request_topic, value)) { + if (participant.publish(request_topic, espp::RtpsParticipant::serialize_uint32_cdr(value))) { logger.info("Published request {} on '{}'", value, request_topic); } else { logger.warn("Failed to publish request {}", value); diff --git a/components/rtps/include/rtps.hpp b/components/rtps/include/rtps.hpp index 45500ef70..25ff3f873 100644 --- a/components/rtps/include/rtps.hpp +++ b/components/rtps/include/rtps.hpp @@ -168,7 +168,7 @@ class RtpsParticipant : public BaseComponent { ReliabilityKind reliability{ ReliabilityKind::BEST_EFFORT}; ///< Reliability QoS advertised for the writer. std::string multicast_group{}; ///< Optional multicast group advertised for this writer and used - ///< by `publish_uint32()` when set. + ///< by `publish()` when set. uint32_t entity_index{0}; ///< Local entity slot used to derive the RTPS entity ID. }; @@ -181,8 +181,10 @@ class RtpsParticipant : public BaseComponent { std::string multicast_group{}; ///< Optional multicast group advertised for this reader and ///< joined on the standard RTPS user-multicast port when set. uint32_t entity_index{0}; ///< Local entity slot used to derive the RTPS entity ID. - std::function on_uint32_sample{ - nullptr}; ///< Callback invoked when a matching temporary UInt32 sample is received. + std::function)> on_sample{ + nullptr}; ///< Callback invoked with the raw CDR-encapsulated serialized payload + ///< (encapsulation header + body) of a matching received sample. The span is only + ///< valid for the duration of the callback. }; /// @brief Cached information about a discovered remote participant. @@ -332,23 +334,25 @@ class RtpsParticipant : public BaseComponent { /// @return Serialized SEDP subscription message for the reader. std::vector build_sedp_subscription_message(const ReaderConfig &reader_config) const; - /// @brief Build a temporary ESPP UInt32 user-data message. + /// @brief Build a standard RTPS user-data DATA message carrying a CDR-encoded sample. /// @param writer_config Local writer configuration used for the topic and writer entity ID. - /// @param value UInt32 sample value to serialize. - /// @param reliability Reliability flag to encode in the temporary payload header. - /// @return Serialized RTPS DATA message containing the temporary ESPP UInt32 payload. - std::vector build_uint32_data_message(const WriterConfig &writer_config, uint32_t value, - ReliabilityKind reliability) const; + /// @param cdr_payload CDR-encapsulated serialized payload (encapsulation header + body) to carry + /// as the DATA submessage serializedPayload. + /// @return Serialized RTPS DATA message containing the sample. + std::vector build_data_message(const WriterConfig &writer_config, + std::span cdr_payload) const; - /// @brief Publish a temporary ESPP UInt32 sample using the configured user-data transport. + /// @brief Publish a CDR-encoded sample on a topic using the configured user-data transport. /// @param topic_name Topic name to publish on. Must match a registered local writer. - /// @param value UInt32 sample value to send. + /// @param cdr_payload CDR-encapsulated serialized payload (encapsulation header + body) to send. /// @return True if at least one send call succeeded, false otherwise. - bool publish_uint32(std::string_view topic_name, uint32_t value); + bool publish(std::string_view topic_name, std::span cdr_payload); /// @brief Serialize a UInt32 value into a standalone CDR payload. /// @param value Value to serialize. /// @return Encapsulated little-endian CDR payload containing the value. + /// @note Convenience helper for the common ROS 2 `std_msgs/msg/UInt32` case; pair it with + /// `publish()` and `deserialize_uint32_cdr()`. Any CDR-encoded type can be published directly. static std::vector serialize_uint32_cdr(uint32_t value); /// @brief Parse a standalone CDR payload containing a UInt32 value. diff --git a/components/rtps/src/rtps.cpp b/components/rtps/src/rtps.cpp index 319a348f6..719c2ab8f 100644 --- a/components/rtps/src/rtps.cpp +++ b/components/rtps/src/rtps.cpp @@ -12,8 +12,6 @@ namespace { constexpr std::array kRtpsMagic{'R', 'T', 'P', 'S'}; -constexpr std::array kUserDataMagic{'E', 'S', 'P', 'P', 'D', 'A', 'T', 'A'}; -constexpr uint8_t kUserDataVersion = 1; constexpr uint16_t kPortBase = 7400; constexpr uint16_t kDomainGain = 250; @@ -322,24 +320,6 @@ bool parse_ipv4(std::string_view address, std::array &octets) { return true; } -void append_string(ByteWriter &writer, std::string_view text) { - writer.append_u16_le(static_cast(text.size())); - writer.append_bytes( - std::span{reinterpret_cast(text.data()), text.size()}); -} - -std::optional read_string(ByteReader &reader) { - uint16_t length = 0; - if (!reader.read_u16_le(length)) { - return std::nullopt; - } - auto span = reader.read_span(length); - if (span.size() != length) { - return std::nullopt; - } - return std::string(reinterpret_cast(span.data()), span.size()); -} - void append_parameter_header(ByteWriter &writer, ParameterId id, uint16_t length) { writer.append_u16_le(static_cast(id)); writer.append_u16_le(length); @@ -1139,26 +1119,19 @@ RtpsParticipant::build_sedp_subscription_message(const ReaderConfig &reader_conf .serialize(); } -std::vector RtpsParticipant::build_uint32_data_message(const WriterConfig &writer_config, - uint32_t value, - ReliabilityKind reliability) const { - ByteWriter payload_writer; - payload_writer.append_chars(kUserDataMagic); - payload_writer.append_u8(kUserDataVersion); - payload_writer.append_u8(static_cast(reliability)); - append_string(payload_writer, writer_config.topic_name); - auto cdr = serialize_uint32_cdr(value); - payload_writer.append_u16_le(static_cast(cdr.size())); - payload_writer.append_bytes(cdr); - +std::vector +RtpsParticipant::build_data_message(const WriterConfig &writer_config, + std::span cdr_payload) const { + // Standard RTPS: the DATA submessage serializedPayload is exactly the CDR-encapsulated sample. + // The topic is identified by the writer GUID (resolved by the receiver via SEDP discovery), so no + // topic name or other framing is embedded in the payload. auto guid = writer_guid(writer_config.entity_index); return build_message(guid_prefix_, {.value = kEntityIdUnknown}, guid.entity_id, - next_user_data_sequence_number(writer_config.entity_index), - payload_writer.take()) + next_user_data_sequence_number(writer_config.entity_index), cdr_payload) .serialize(); } -bool RtpsParticipant::publish_uint32(std::string_view topic_name, uint32_t value) { +bool RtpsParticipant::publish(std::string_view topic_name, std::span cdr_payload) { WriterConfig writer_config; { std::lock_guard lock(mutex_); @@ -1176,10 +1149,7 @@ bool RtpsParticipant::publish_uint32(std::string_view topic_name, uint32_t value logger_.warn("Reliable user-data retransmission is not implemented yet; sending best-effort"); } - auto encoded_reliability = writer_config.reliability == ReliabilityKind::RELIABLE - ? ReliabilityKind::BEST_EFFORT - : writer_config.reliability; - auto payload = build_uint32_data_message(writer_config, value, encoded_reliability); + auto payload = build_data_message(writer_config, cdr_payload); if (!user_unicast_receiver_) { return false; @@ -1471,48 +1441,48 @@ bool RtpsParticipant::handle_user_message(std::vector &data, const Sock for (const auto &submessage : message->submessages) { bool valid_data = false; auto data_view = parse_data_submessage(submessage, valid_data); - if (!valid_data || data_view.serialized_payload.size() < kUserDataMagic.size() + 2 || - !std::equal(kUserDataMagic.begin(), kUserDataMagic.end(), - data_view.serialized_payload.begin())) { - continue; - } - - ByteReader reader(data_view.serialized_payload); - std::array magic{}; - uint8_t version = 0; - uint8_t reliability = 0; - if (!reader.read_bytes(std::span{magic.data(), magic.size()}) || - !reader.read_u8(version) || !reader.read_u8(reliability)) { - continue; - } - auto topic_name = read_string(reader); - uint16_t payload_length = 0; - if (version != kUserDataVersion || !topic_name || !reader.read_u16_le(payload_length)) { - continue; - } - auto payload = reader.read_span(payload_length); - auto maybe_value = deserialize_uint32_cdr(payload); - if (!maybe_value) { + if (!valid_data) { continue; } - if (static_cast(reliability) == ReliabilityKind::RELIABLE) { - logger_.warn( - "Received reliable topic '{}' from {}, but ACKNACK/HEARTBEAT is not implemented yet", - *topic_name, sender); - } - - std::vector> callbacks; + // Standard RTPS: the sample's topic is identified by the writer GUID, which we resolve through + // SEDP discovery state. Build the remote writer GUID from the message prefix + DATA writerId, + // then look up its topic and reliability among discovered writers, and collect the matching + // local reader callbacks under a single lock. + Guid remote_writer_guid{.prefix = message->header.guid_prefix, + .entity_id = data_view.writer_id}; + std::string topic_name; + bool writer_is_reliable = false; + std::vector)>> callbacks; { std::lock_guard lock(mutex_); + auto writer = std::find_if( + discovered_writers_.begin(), discovered_writers_.end(), + [&remote_writer_guid](const auto &w) { return w.guid == remote_writer_guid; }); + if (writer == discovered_writers_.end()) { + // Sample arrived before the writer was discovered via SEDP; drop it (best-effort). + continue; + } + topic_name = writer->topic_name; + writer_is_reliable = writer->reliability == ReliabilityKind::RELIABLE; for (const auto &reader_config : readers_) { - if (reader_config.topic_name == *topic_name && reader_config.on_uint32_sample) { - callbacks.push_back(reader_config.on_uint32_sample); + if (reader_config.topic_name == topic_name && reader_config.on_sample) { + callbacks.push_back(reader_config.on_sample); } } } + + if (callbacks.empty()) { + continue; + } + if (writer_is_reliable) { + logger_.warn("Received sample on reliable topic '{}' from {}, but ACKNACK/HEARTBEAT is not " + "implemented " + "yet", + topic_name, sender); + } for (const auto &callback : callbacks) { - callback(*maybe_value); + callback(data_view.serialized_payload); } } return false; From 58bce6d40d490e0439c342f5559041e14d2c6d32 Mon Sep 17 00:00:00 2001 From: William Emfinger Date: Thu, 18 Jun 2026 09:03:39 -0500 Subject: [PATCH 22/32] clean up some more and fix some issues --- components/rtps/example/main/rtps_example.cpp | 36 ++- components/rtps/include/rtps.hpp | 12 - components/rtps/src/rtps.cpp | 21 -- python/README.md | 8 +- python/rtps_host.py | 227 ++++++++++++------ 5 files changed, 186 insertions(+), 118 deletions(-) diff --git a/components/rtps/example/main/rtps_example.cpp b/components/rtps/example/main/rtps_example.cpp index 97e03ce8a..b5d9217bf 100644 --- a/components/rtps/example/main/rtps_example.cpp +++ b/components/rtps/example/main/rtps_example.cpp @@ -1,8 +1,13 @@ #include #include #include +#include +#include +#include #include +#include +#include "cdr.hpp" #include "logger.hpp" #include "rtps.hpp" #include "wifi_sta.hpp" @@ -12,6 +17,24 @@ using namespace std::chrono_literals; namespace { constexpr std::string_view kTypeName = "std_msgs/msg/UInt32"; +// Thin wrappers over the `cdr` component for the std_msgs/msg/UInt32 payload used by this example. +// The rtps `publish()` / `on_sample` API works with any CDR-encoded type, so serialization lives in +// the application rather than the participant. +std::vector serialize_uint32(uint32_t value) { + espp::CdrWriter writer; + writer.write(value); + return writer.take_buffer(); +} + +std::optional deserialize_uint32(std::span cdr) { + espp::CdrReader reader(cdr); + uint32_t value = 0; + if (!reader.valid() || !reader.read(value)) { + return std::nullopt; + } + return value; +} + bool run_local_protocol_checks(espp::Logger &logger, const espp::RtpsParticipant &participant) { auto announce_message = participant.build_announce_message(); auto parsed_message = espp::RtpsParticipant::Message::parse(announce_message); @@ -48,8 +71,8 @@ bool run_local_protocol_checks(espp::Logger &logger, const espp::RtpsParticipant parsed_subscription_message->submessages.size()); } - auto uint32_payload = espp::RtpsParticipant::serialize_uint32_cdr(42); - auto maybe_value = espp::RtpsParticipant::deserialize_uint32_cdr(uint32_payload); + auto uint32_payload = serialize_uint32(42); + auto maybe_value = deserialize_uint32(uint32_payload); if (!maybe_value || *maybe_value != 42) { logger.error("UInt32 CDR round trip failed"); return false; @@ -153,7 +176,7 @@ extern "C" void app_main(void) { .entity_index = 0, .on_sample = [&logger, &response_count, &last_sent_request](std::span cdr) { - auto value = espp::RtpsParticipant::deserialize_uint32_cdr(cdr); + auto value = deserialize_uint32(cdr); if (!value) { return; } @@ -179,14 +202,13 @@ extern "C" void app_main(void) { .on_sample = [&logger, &request_count, &response_topic, &participant_ptr](std::span cdr) { - auto value = espp::RtpsParticipant::deserialize_uint32_cdr(cdr); + auto value = deserialize_uint32(cdr); if (!value) { return; } request_count++; logger.info("Received request {}, sending response", *value); - if (!participant_ptr->publish(response_topic, - espp::RtpsParticipant::serialize_uint32_cdr(*value))) { + if (!participant_ptr->publish(response_topic, serialize_uint32(*value))) { logger.warn("Failed to publish response {}", *value); } }, @@ -240,7 +262,7 @@ extern "C" void app_main(void) { auto value = next_request_value.fetch_add(1); last_sent_request = value; - if (participant.publish(request_topic, espp::RtpsParticipant::serialize_uint32_cdr(value))) { + if (participant.publish(request_topic, serialize_uint32(value))) { logger.info("Published request {} on '{}'", value, request_topic); } else { logger.warn("Failed to publish request {}", value); diff --git a/components/rtps/include/rtps.hpp b/components/rtps/include/rtps.hpp index 25ff3f873..4fcdddb86 100644 --- a/components/rtps/include/rtps.hpp +++ b/components/rtps/include/rtps.hpp @@ -348,18 +348,6 @@ class RtpsParticipant : public BaseComponent { /// @return True if at least one send call succeeded, false otherwise. bool publish(std::string_view topic_name, std::span cdr_payload); - /// @brief Serialize a UInt32 value into a standalone CDR payload. - /// @param value Value to serialize. - /// @return Encapsulated little-endian CDR payload containing the value. - /// @note Convenience helper for the common ROS 2 `std_msgs/msg/UInt32` case; pair it with - /// `publish()` and `deserialize_uint32_cdr()`. Any CDR-encoded type can be published directly. - static std::vector serialize_uint32_cdr(uint32_t value); - - /// @brief Parse a standalone CDR payload containing a UInt32 value. - /// @param data Encapsulated CDR payload bytes. - /// @return Parsed UInt32 value on success, or `std::nullopt` if the payload is invalid. - static std::optional deserialize_uint32_cdr(std::span data); - /// @brief Compute the standard RTPS UDP port mapping for a domain/participant pair. /// @param domain_id RTPS domain ID. /// @param participant_id RTPS participant ID. diff --git a/components/rtps/src/rtps.cpp b/components/rtps/src/rtps.cpp index 719c2ab8f..2a61530a3 100644 --- a/components/rtps/src/rtps.cpp +++ b/components/rtps/src/rtps.cpp @@ -1189,27 +1189,6 @@ int64_t RtpsParticipant::next_user_data_sequence_number(uint32_t entity_index) c return current; } -std::vector RtpsParticipant::serialize_uint32_cdr(uint32_t value) { - espp::CdrWriter writer({ - .encapsulation = espp::CdrEncapsulation::CDR_LE, - .include_encapsulation = true, - }); - writer.write(value); - return writer.take_buffer(); -} - -std::optional RtpsParticipant::deserialize_uint32_cdr(std::span data) { - espp::CdrReader reader(data); - if (!reader.valid()) { - return std::nullopt; - } - uint32_t value = 0; - if (!reader.read(value)) { - return std::nullopt; - } - return value; -} - RtpsParticipant::PortMapping RtpsParticipant::compute_port_mapping(uint16_t domain_id, uint16_t participant_id) { auto base = static_cast(kPortBase) + static_cast(kDomainGain) * domain_id; diff --git a/python/README.md b/python/README.md index 9ea454864..c54b698e6 100644 --- a/python/README.md +++ b/python/README.md @@ -49,9 +49,11 @@ This section gives a brief overview of what the scripts in this folder do. window. - `rtps_host.py`: A pure-stdlib host-side RTPS harness for discovering an ESPP `RtpsParticipant`, printing SPDP/SEDP metadata, and optionally publishing or - receiving the current temporary `UInt32` user-data payloads without needing - Python bindings. It now follows endpoint-advertised user-data multicast - locators, joining matching subscribed-topic multicast groups dynamically. + receiving standard CDR-over-RTPS `UInt32` user-data samples (routed by writer + GUID via SEDP) without needing Python bindings. It follows endpoint-advertised + user-data multicast locators, joining matching subscribed-topic multicast + groups dynamically. Run `python rtps_host.py --self-test` to validate the + wire-format encoders/decoders against the firmware with no network I/O. - `cobs_demo.py`: Demonstration of ESPP COBS functionality with native Python data types. Shows ESPP encoding/decoding, cross-library compatibility with the cobs-python library, and practical usage examples. Includes design differences explanation and validation diff --git a/python/rtps_host.py b/python/rtps_host.py index 5cd6d4beb..533b5d3c8 100644 --- a/python/rtps_host.py +++ b/python/rtps_host.py @@ -1,14 +1,18 @@ #!/usr/bin/env python3 """Simple host-side RTPS test harness for the ESPP RTPS component. -This script speaks the current ESPP RTPS discovery wire format plus the -temporary ``UInt32`` user-data payload used by ``RtpsParticipant`` today. It is -useful for: +This script speaks the ESPP RTPS discovery wire format plus the standard +CDR-over-RTPS user-data path used by ``RtpsParticipant``. It is useful for: 1. discovering an embedded ESPP RTPS participant from a PC/host, 2. inspecting SPDP/SEDP announcements, and -3. sending or receiving ``std_msgs/msg/UInt32``-style test samples over the - temporary ESPP user-data path. +3. sending or receiving ``std_msgs/msg/UInt32``-style test samples. User samples + are standard RTPS ``DATA`` submessages whose serializedPayload is the raw + CDR-encapsulated sample; the topic is identified by the writer GUID resolved + through SEDP discovery (no ESPP-specific payload framing). + +Run ``python rtps_host.py --self-test`` to validate the wire-format encoders and +decoders against the firmware's expectations without any network I/O. It uses only the Python standard library, so it does not require Python bindings or a rebuilt host ``lib/`` tree. @@ -30,8 +34,6 @@ RTPS_MAGIC = b"RTPS" PL_CDR_LE = b"\x00\x03\x00\x00" -USER_DATA_MAGIC = b"ESPPDATA" -USER_DATA_VERSION = 1 PORT_BASE = 7400 DOMAIN_GAIN = 250 @@ -47,8 +49,6 @@ RTPS_QOS_RELIABILITY_BEST_EFFORT = 1 RTPS_QOS_RELIABILITY_RELIABLE = 2 -USER_DATA_RELIABILITY_BEST_EFFORT = 0 -USER_DATA_RELIABILITY_RELIABLE = 1 KIND_UDP_V4 = 1 VENDOR_ID = b"\xca\xfe" @@ -179,12 +179,14 @@ def reliability_to_name(reliable: bool) -> str: return "reliable" if reliable else "best-effort" -def encode_user_data_reliability(reliable: bool) -> int: - return USER_DATA_RELIABILITY_RELIABLE if reliable else USER_DATA_RELIABILITY_BEST_EFFORT +def ntp_fraction_from_nanoseconds(nanoseconds: int) -> int: + # RTPS Duration_t/Time_t use NTP fraction units of 1/2^32 s, not nanoseconds. + return (nanoseconds << 32) // 1_000_000_000 -def decode_user_data_reliability(encoded: int) -> str: - return "reliable" if encoded == USER_DATA_RELIABILITY_RELIABLE else "best-effort" +def padded_parameter_length(length: int) -> int: + # RTPS PL_CDR requires each parameterLength to be a multiple of 4. + return (length + 3) & ~3 def compute_port_mapping(domain_id: int, participant_id: int) -> PortMapping: @@ -262,13 +264,14 @@ def append_parameter_bool(buffer: bytearray, pid: int, value: bool) -> None: def append_parameter_duration(buffer: bytearray, pid: int, seconds: int, nanoseconds: int) -> None: append_parameter_header(buffer, pid, 8) - buffer.extend(struct.pack(" bytes: + # Locator_t.kind/.port are little-endian in PL_CDR_LE; only the 16-byte address is raw bytes. locator = bytearray(24) - struct.pack_into(">I", locator, 0, KIND_UDP_V4) - struct.pack_into(">I", locator, 4, port) + struct.pack_into(" None: encoded = text.encode("utf-8") - append_parameter_header(buffer, pid, 4 + len(encoded) + 1) + # parameterLength must be a multiple of 4 and includes the trailing CDR padding. + append_parameter_header(buffer, pid, padded_parameter_length(4 + len(encoded) + 1)) buffer.extend(struct.pack(" None: def append_parameter_octet_sequence(buffer: bytearray, pid: int, payload: bytes) -> None: - append_parameter_header(buffer, pid, 4 + len(payload)) + append_parameter_header(buffer, pid, padded_parameter_length(4 + len(payload))) buffer.extend(struct.pack(" None: append_parameter_header(buffer, PID_RELIABILITY, 12) kind = RTPS_QOS_RELIABILITY_RELIABLE if reliable else RTPS_QOS_RELIABILITY_BEST_EFFORT buffer.extend(struct.pack(" None: @@ -309,7 +319,13 @@ def append_parameter_durability(buffer: bytearray) -> None: def append_parameter_liveliness(buffer: bytearray) -> None: append_parameter_header(buffer, PID_LIVELINESS, 12) buffer.extend(struct.pack(" None: @@ -423,10 +439,10 @@ def parse_octet_sequence(value: Optional[bytes]) -> Optional[bytes]: def parse_locator(value: Optional[bytes]) -> tuple[str, int]: if value is None or len(value) != 24: return ("0.0.0.0", 0) - kind = struct.unpack_from(">I", value, 0)[0] + kind = struct.unpack_from("I", value, 4)[0] + port = struct.unpack_from(" None: self.next_discovery_send = 0.0 self.next_publish_send = 0.0 self.last_no_participant_log = 0.0 + self.last_unknown_writer_log = 0.0 def _create_bound_udp_socket(self, port: int) -> socket.socket: sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM, socket.IPPROTO_UDP) @@ -548,7 +565,12 @@ def _create_user_multicast_socket(self) -> socket.socket: # Some platforms expose SO_REUSEPORT but reject setting it; this is # a best-effort optimization and is not required for correctness. pass - sock.bind((self.args.bind_address, self.ports.user_multicast)) + # Bind to INADDR_ANY, not a unicast address: multicast datagrams are addressed to the group + # (e.g. 239.255.0.11), so a socket bound to a specific unicast interface address will not + # receive them on Linux (and unreliably on macOS). Delivery is decided by the joined + # group(s) + port. This socket may join several user-data groups, so binding to a single + # group address is not an option. + sock.bind(("", self.ports.user_multicast)) sock.setblocking(False) return sock @@ -683,24 +705,15 @@ def build_sedp_subscription_message(self, reader: ReaderConfig) -> bytes: payload, ) - def build_uint32_data_message(self, writer: WriterConfig, value: int) -> bytes: - payload = bytearray() - payload.extend(USER_DATA_MAGIC) - payload.append(USER_DATA_VERSION) - payload.append(encode_user_data_reliability(writer.reliable)) - topic_name = writer.topic_name.encode("utf-8") - payload.extend(struct.pack(" bytes: + # Standard RTPS: the DATA serializedPayload is exactly the CDR-encapsulated sample. writer_entity_id = entity_id_for_index(writer.entity_index, USER_WRITER_NO_KEY_KIND) return build_rtps_message( self.guid_prefix, ENTITY_ID_UNKNOWN, writer_entity_id, self._next_sequence(writer_entity_id), - bytes(payload), + cdr_payload, ) def send_spdp_announce_now(self) -> None: @@ -765,7 +778,7 @@ def _build_user_targets(self, writer: WriterConfig) -> List[Tuple[str, int]]: return targets def _publish_value(self, writer: WriterConfig, value: int, target: Optional[tuple[str, int]] = None) -> bool: - payload = self.build_uint32_data_message(writer, value) + payload = self.build_data_message(writer, serialize_uint32_cdr(value)) if target is not None: self.user_unicast_sock.sendto(payload, target) return True @@ -777,7 +790,7 @@ def _publish_value(self, writer: WriterConfig, value: int, target: Optional[tupl return True def handle_metatraffic_packet(self, packet: bytes, sender_ip: str) -> None: - for writer_id, serialized_payload in parse_rtps_data_messages(packet): + for _guid_prefix, writer_id, serialized_payload in parse_rtps_data_messages(packet): parameters = parse_parameter_list(serialized_payload) if not parameters: continue @@ -873,49 +886,43 @@ def _handle_sedp(self, parameters: List[tuple[int, bytes]], sender_ip: str, is_r ) def handle_user_packet(self, packet: bytes, sender_ip: str, sender_port: int) -> None: - for writer_id, serialized_payload in parse_rtps_data_messages(packet): - if not serialized_payload.startswith(USER_DATA_MAGIC) or len(serialized_payload) < len(USER_DATA_MAGIC) + 2: - continue - offset = len(USER_DATA_MAGIC) - version = serialized_payload[offset] - reliability = serialized_payload[offset + 1] - offset += 2 - if version != USER_DATA_VERSION: - continue - if offset + 2 > len(serialized_payload): - continue - topic_length = struct.unpack_from(" len(serialized_payload): - continue - topic_name = serialized_payload[offset : offset + topic_length].decode("utf-8", errors="replace") - offset += topic_length - if offset + 2 > len(serialized_payload): + subscribed_topics = {reader.topic_name for reader in self.local_readers} + for guid_prefix, writer_id, serialized_payload in parse_rtps_data_messages(packet): + # Standard RTPS: resolve the topic from the writer GUID via SEDP discovery state. + writer_guid = guid_prefix + writer_id + writer = self.discovered_writers.get(writer_guid) + if writer is None: + # Sample arrived before its writer was discovered via SEDP; drop it (best-effort). + # Surface it (rate-limited) so a missing SEDP exchange is visible rather than silent. + now = time.monotonic() + if now - self.last_unknown_writer_log > 2.0: + log( + f"[data] received {len(serialized_payload)}-byte sample from UNDISCOVERED " + f"writer {guid_to_string(writer_guid)} at {sender_ip}:{sender_port}; cannot " + f"route without SEDP (discovered_writers={len(self.discovered_writers)})" + ) + self.last_unknown_writer_log = now continue - payload_length = struct.unpack_from(" len(serialized_payload): + topic_name = writer.topic_name + if topic_name not in subscribed_topics: continue - maybe_value = deserialize_uint32_cdr(serialized_payload[offset : offset + payload_length]) + maybe_value = deserialize_uint32_cdr(serialized_payload) if maybe_value is None: continue - reliability_name = decode_user_data_reliability(reliability) log( - f"[data] topic='{topic_name}' value={maybe_value} reliability={reliability_name} " + f"[data] topic='{topic_name}' value={maybe_value} reliability={writer.reliability} " f"from {sender_ip}:{sender_port} writer={hex_string(writer_id)}" ) if self.args.echo_received and self.local_writers: - subscribed_topics = {reader.topic_name for reader in self.local_readers} - if topic_name in subscribed_topics: - writer = self.local_writers[0] - if self._publish_value(writer, maybe_value): - log(f"[echo] responded with value={maybe_value} on '{writer.topic_name}'") - else: - self._publish_value(writer, maybe_value, (sender_ip, sender_port)) - log( - f"[echo] responded with value={maybe_value} on '{writer.topic_name}' " - f"to {sender_ip}:{sender_port}" - ) + out_writer = self.local_writers[0] + if self._publish_value(out_writer, maybe_value): + log(f"[echo] responded with value={maybe_value} on '{out_writer.topic_name}'") + else: + self._publish_value(out_writer, maybe_value, (sender_ip, sender_port)) + log( + f"[echo] responded with value={maybe_value} on '{out_writer.topic_name}' " + f"to {sender_ip}:{sender_port}" + ) def run(self) -> None: start_time = time.monotonic() @@ -990,11 +997,13 @@ def close(self) -> None: log(f"[close] ignoring socket close failure for {sock!r}: {exc}") -def parse_rtps_data_messages(packet: bytes) -> List[tuple[bytes, bytes]]: +def parse_rtps_data_messages(packet: bytes) -> List[tuple[bytes, bytes, bytes]]: + """Return (guid_prefix, writer_id, serialized_payload) for each DATA submessage.""" if len(packet) < 20 or not packet.startswith(RTPS_MAGIC): return [] + guid_prefix = packet[8:20] offset = 20 - messages: List[tuple[bytes, bytes]] = [] + messages: List[tuple[bytes, bytes, bytes]] = [] while offset + 4 <= len(packet): kind = packet[offset] flags = packet[offset + 1] @@ -1014,10 +1023,75 @@ def parse_rtps_data_messages(packet: bytes) -> List[tuple[bytes, bytes]]: continue writer_id = payload[8:12] serialized_payload = payload[20:] - messages.append((writer_id, serialized_payload)) + messages.append((guid_prefix, writer_id, serialized_payload)) return messages +def run_self_test() -> int: + """Validate the wire-format encoders/decoders against firmware expectations (no network I/O).""" + failures: List[str] = [] + + def check(name: str, condition: bool) -> None: + log(f" [{'PASS' if condition else 'FAIL'}] {name}") + if not condition: + failures.append(name) + + # Locators: kind/port are little-endian, address is raw network-order bytes; round-trips. + loc = locator_bytes("192.168.1.5", 7411) + check("locator kind is little-endian", loc[:4] == struct.pack(" 16 + append_parameter_string_cdr(params, PID_TYPE_NAME, "std_msgs::msg::dds_::UInt32_") + append_parameter_octet_sequence(params, PID_USER_DATA, b"enclave=/;") # body 4+10=14 -> 16 + offset = 0 + aligned = True + while offset + 4 <= len(params): + pid, length = struct.unpack_from(" argparse.Namespace: parser = argparse.ArgumentParser( description=( @@ -1115,6 +1189,9 @@ def parse_args() -> argparse.Namespace: def main() -> int: + # Handle --self-test before full argument parsing so it needs no network/address configuration. + if "--self-test" in sys.argv: + return run_self_test() args = parse_args() harness = RtpsHostHarness(args) harness.run() From 578de256e194fc30479723a6fca80796058cdefe Mon Sep 17 00:00:00 2001 From: William Emfinger Date: Thu, 18 Jun 2026 09:10:38 -0500 Subject: [PATCH 23/32] make the crd de-/serialization format clearer for the example --- components/rtps/example/main/rtps_example.cpp | 14 ++++++++++++-- 1 file changed, 12 insertions(+), 2 deletions(-) diff --git a/components/rtps/example/main/rtps_example.cpp b/components/rtps/example/main/rtps_example.cpp index b5d9217bf..a18a56fd4 100644 --- a/components/rtps/example/main/rtps_example.cpp +++ b/components/rtps/example/main/rtps_example.cpp @@ -21,13 +21,23 @@ constexpr std::string_view kTypeName = "std_msgs/msg/UInt32"; // The rtps `publish()` / `on_sample` API works with any CDR-encoded type, so serialization lives in // the application rather than the participant. std::vector serialize_uint32(uint32_t value) { - espp::CdrWriter writer; + // The encapsulation options below are the CdrWriter defaults (little-endian CDR with a 4-byte + // encapsulation header); shown explicitly for clarity about the on-the-wire format. + espp::CdrWriter writer({ + .encapsulation = espp::CdrEncapsulation::CDR_LE, + .include_encapsulation = true, + }); writer.write(value); return writer.take_buffer(); } std::optional deserialize_uint32(std::span cdr) { - espp::CdrReader reader(cdr); + // The config below matches the CdrReader defaults (expects a little-endian CDR encapsulation + // header); shown explicitly to mirror serialize_uint32() above. + espp::CdrReader reader(cdr, { + .expect_encapsulation = true, + .default_encapsulation = espp::CdrEncapsulation::CDR_LE, + }); uint32_t value = 0; if (!reader.valid() || !reader.read(value)) { return std::nullopt; From 10d216e3664653cb401709be6f390352ea684155 Mon Sep 17 00:00:00 2001 From: William Emfinger Date: Thu, 18 Jun 2026 09:18:53 -0500 Subject: [PATCH 24/32] improve hash function --- components/rtps/src/rtps.cpp | 18 +++++++++++++++++- 1 file changed, 17 insertions(+), 1 deletion(-) diff --git a/components/rtps/src/rtps.cpp b/components/rtps/src/rtps.cpp index 2a61530a3..171efb49d 100644 --- a/components/rtps/src/rtps.cpp +++ b/components/rtps/src/rtps.cpp @@ -93,6 +93,19 @@ enum class ParameterId : uint16_t { PID_HISTORY = 0x0040, }; +// 64-bit FNV-1a hash. Used to derive the node-name portion of the GUID prefix with a full 64 bits +// of entropy regardless of the platform's size_t width. std::hash is only 32-bit on +// the 32-bit ESP32, which made bytes 8..11 of the prefix a repeated copy of bytes 4..7 (the shift +// `hash >> (8 * i)` for i >= 4 was undefined behavior on a 32-bit value). +uint64_t fnv1a_64(std::string_view text) { + uint64_t hash = 1469598103934665603ull; // FNV-1a 64-bit offset basis + for (unsigned char c : text) { + hash ^= c; + hash *= 1099511628211ull; // FNV-1a 64-bit prime + } + return hash; +} + class ByteWriter { public: void append_bytes(std::span bytes) { @@ -818,7 +831,10 @@ RtpsParticipant::Message::parse(std::span data) { RtpsParticipant::RtpsParticipant(const Config &config) : BaseComponent({.tag = "RtpsParticipant", .level = config.log_level}) , config_(config) { - auto hash = std::hash{}(config_.node_name); + // GUID prefix layout: bytes 0..1 = participant_id, 2..3 = domain_id, 4..11 = 64-bit node-name + // hash. Uniqueness across participants on one host relies on distinct participant_ids; the + // node-name hash distinguishes different nodes/applications. + uint64_t hash = fnv1a_64(config_.node_name); guid_prefix_.value[0] = config_.participant_id & 0xff; guid_prefix_.value[1] = (config_.participant_id >> 8) & 0xff; guid_prefix_.value[2] = config_.domain_id & 0xff; From c5696f1d18238e6b70260f9fc33185c2897c1649 Mon Sep 17 00:00:00 2001 From: William Emfinger Date: Thu, 18 Jun 2026 09:31:53 -0500 Subject: [PATCH 25/32] add cdr and rtps to xplat lib (c++) and update pybind11 dep to 3.0.4 --- lib/CMakeLists.txt | 2 +- lib/espp.cmake | 4 ++++ 2 files changed, 5 insertions(+), 1 deletion(-) diff --git a/lib/CMakeLists.txt b/lib/CMakeLists.txt index 843a34763..36f269c42 100644 --- a/lib/CMakeLists.txt +++ b/lib/CMakeLists.txt @@ -9,7 +9,7 @@ include(cmake/CPM.cmake) CPMAddPackage( NAME pybind11 GIT_REPOSITORY https://github.com/pybind/pybind11.git - VERSION 2.13.6 + VERSION 3.0.4 DOWNLOAD_ONLY YES ) if(pybind11_ADDED) diff --git a/lib/espp.cmake b/lib/espp.cmake index 95bd2d761..e630ed5ea 100644 --- a/lib/espp.cmake +++ b/lib/espp.cmake @@ -19,6 +19,7 @@ set(ESPP_EXTERNAL_INCLUDES_SEPARATE set(ESPP_INCLUDES ${ESPP_COMPONENTS}/base_component/include ${ESPP_COMPONENTS}/base_peripheral/include + ${ESPP_COMPONENTS}/cdr/include ${ESPP_COMPONENTS}/cobs/include ${ESPP_COMPONENTS}/color/include ${ESPP_COMPONENTS}/csv/include @@ -33,6 +34,7 @@ set(ESPP_INCLUDES ${ESPP_COMPONENTS}/math/include ${ESPP_COMPONENTS}/ndef/include ${ESPP_COMPONENTS}/pid/include + ${ESPP_COMPONENTS}/rtps/include ${ESPP_COMPONENTS}/rtsp/include ${ESPP_COMPONENTS}/serialization/include ${ESPP_COMPONENTS}/tabulate/include @@ -44,6 +46,7 @@ set(ESPP_INCLUDES ) set(ESPP_SOURCES + ${ESPP_COMPONENTS}/cdr/src/cdr.cpp ${ESPP_COMPONENTS}/cobs/src/cobs.cpp ${ESPP_COMPONENTS}/cobs/src/cobs_stream.cpp ${ESPP_COMPONENTS}/color/src/color.cpp @@ -53,6 +56,7 @@ set(ESPP_SOURCES ${ESPP_COMPONENTS}/filters/src/lowpass_filter.cpp ${ESPP_COMPONENTS}/filters/src/simple_lowpass_filter.cpp ${ESPP_COMPONENTS}/joystick/src/joystick.cpp + ${ESPP_COMPONENTS}/rtps/src/rtps.cpp ${ESPP_COMPONENTS}/rtsp/src/rtcp_packet.cpp ${ESPP_COMPONENTS}/rtsp/src/rtp_packet.cpp ${ESPP_COMPONENTS}/rtsp/src/rtsp_client.cpp From b7fe2e60b07c3c3785c1bf1aaec5df1eef458c42 Mon Sep 17 00:00:00 2001 From: William Emfinger Date: Thu, 18 Jun 2026 22:26:29 -0500 Subject: [PATCH 26/32] add to include --- lib/include/espp.hpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/lib/include/espp.hpp b/lib/include/espp.hpp index f6006e776..064148c5d 100644 --- a/lib/include/espp.hpp +++ b/lib/include/espp.hpp @@ -15,6 +15,7 @@ extern "C" { #include "base_component.hpp" #include "bezier.hpp" #include "butterworth_filter.hpp" +#include "cdr.hpp" #include "cobs.hpp" #include "cobs_stream.hpp" #include "color.hpp" @@ -43,6 +44,7 @@ extern "C" { #include "rtp_depacketizer.hpp" #include "rtp_packetizer.hpp" #include "rtp_types.hpp" +#include "rtps.hpp" #include "rtsp_client.hpp" #include "rtsp_server.hpp" #include "serialization.hpp" From 2f2c31c0ebdf1ddf97060355e1aabf6e6b5433b8 Mon Sep 17 00:00:00 2001 From: William Emfinger Date: Thu, 18 Jun 2026 23:00:26 -0500 Subject: [PATCH 27/32] significantly rework python bindings to automate manual rework for future me; expose cdr and rtps components via manually-written shims rather than litgen --- lib/README.md | 57 +- lib/autogenerate_bindings.py | 251 +- lib/espp.cmake | 2 + lib/fix_generated_bindings.py | 154 + lib/python_bindings/cdr_bindings.cpp | 246 ++ lib/python_bindings/espp/__init__.pyi | 3832 ++++++++++++++----------- lib/python_bindings/module.cpp | 7 + lib/python_bindings/pybind_espp.cpp | 573 ++-- lib/python_bindings/rtps_bindings.cpp | 212 ++ 9 files changed, 3452 insertions(+), 1882 deletions(-) create mode 100644 lib/fix_generated_bindings.py create mode 100644 lib/python_bindings/cdr_bindings.cpp create mode 100644 lib/python_bindings/rtps_bindings.cpp diff --git a/lib/README.md b/lib/README.md index 48379e4da..52a00d2cc 100644 --- a/lib/README.md +++ b/lib/README.md @@ -92,29 +92,42 @@ pip install -r requirements.txt ```console # start the environment source env/bin/activate +# configure the build once so the qualification fixer has the exact compile flags: +cmake -DCMAKE_EXPORT_COMPILE_COMMANDS=ON -S . -B build python autogenerate_bindings.py ``` -Note: after you autogenerate the bindings, you will need to manually modify the -generated code in -[./python_bindings/pybind_espp.cpp](./python_bindings/pybind_espp.cpp) slightly -(at least until the underlying bugs in litgen/srcmlcpp are fixed): -1. You must fix the `RangeMapper::` to be `RangeMapper::` and - `RangeMapper::` where appropriate. -2. You must fix the `pyClassRangeMapper,` to be `pyClassRangeMapper_int,` and - `pyClassRangeMapper_float,` where appropriate (1 place each). -3. You must fix the `Bezier::` to be properly templated on `espp::Vector2f` - (`Bezier::`). -4. You must fix the `pyClassBezier` to be instead `pyClassBezier_espp_Vector2f`. -5. You must fix the `Vector2d` generated template code for both `int` and - `float` to ensure that the template type is always provided. -6. Srcml currently has an [issue with inner - structs](https://github.com/srcML/srcML/issues/2033). This means that for - `Bezier`, `Gaussian`, `Logger`, `Pid`, `Socket`, `Task`, `Timer`, - `TcpSocket`, `UdpSocket`, and `Joystick`, litgen will improperly generate an - `implicit default constructor`. You you can update the template parameter for - it to have the appropriate `const espp::::Config&` template - parameter. Note that for some classes, they may have multiple config - options - so for `Bezier`, `Task`, `Timer`, etc. you will want to create - overloads which target each of the config types. +**No manual editing of the generated file is required.** The fixups that used to +be applied by hand are now fully automated: + +- `autogenerate_bindings.py` strips C++20 `requires`-clauses at parse time (so + `Vector2d` and friends parse), then post-processes the generated + `pybind_espp.cpp` to fix the litgen/srcmlcpp bugs: bogus implicit default + constructors (`Logger`, `Task`, the RTSP packetizers, ...), template-class + nested args (`RangeMapper`, `Bezier`, `Vector2d`), + static/instance method duplicates (`Task::get_id`), and `std::shared_ptr` + holders (the RTP packetizer hierarchy, `JpegFrame`). +- It then runs [`fix_generated_bindings.py`](./fix_generated_bindings.py), which + compiles the generated file and applies clang's own + `'Y'; did you mean 'espp::X::Y'?` nested-scope qualifications (the previously + *undocumented* RTSP/rtps edits) plus `def_readwrite` → `def_readonly` for + non-copyable members, iterating until the file compiles cleanly. It needs the + `build/compile_commands.json` from the cmake step above; if it is missing, + generation prints a hint and you can run `python fix_generated_bindings.py` + after configuring the build. + +The litgen dependency is unpinned and recent versions (0.20–0.22) regressed +nested-scope/template generation, which is why this automation is needed. + +### Hand-written components (`cdr`, `rtps`) + +`cdr` and `rtps` are **not** generated by litgen — they are bound by hand in +[./python_bindings/cdr_bindings.cpp](./python_bindings/cdr_bindings.cpp) and +[./python_bindings/rtps_bindings.cpp](./python_bindings/rtps_bindings.cpp) +(registered via `py_init_cdr` / `py_init_rtps` in `module.cpp`). litgen cannot +bind them usefully (output-reference reads, non-owning `std::span`, +`std::function` callbacks), and the hand-written shims give a clean, GIL-correct +Python API (`CdrWriter`/`CdrReader`, `RtpsParticipant` with +`publish(topic, bytes)` and `ReaderConfig.on_sample = callable(bytes)`). Edit +those files directly; regeneration never touches them. diff --git a/lib/autogenerate_bindings.py b/lib/autogenerate_bindings.py index 3d3624ec7..65904a47c 100644 --- a/lib/autogenerate_bindings.py +++ b/lib/autogenerate_bindings.py @@ -1,6 +1,208 @@ import litgen from srcmlcpp import SrcmlcppOptions import os +import re + +# NOTE: cdr.hpp is intentionally NOT generated here. srcmlcpp cannot parse it, and even when coerced +# the generated CdrReader API is unusable (output-reference reads can't return values, and the +# non-owning std::span would dangle). It is bound by hand instead in +# python_bindings/cdr_bindings.cpp (registered via py_init_cdr in module.cpp). + + +# //////////////////////////////////////////////////////////////////////////////////////////////// +# Parse-time preprocessing +# //////////////////////////////////////////////////////////////////////////////////////////////// +# srcmlcpp mis-parses a class when its members carry a C++20 trailing `requires` clause (e.g. +# `T angle(...) const requires std::is_floating_point::value { ... }` in Vector2d). The class +# scope is lost, so litgen emits members as stray free functions and does not specialize the +# template class at all. The constraint is irrelevant to bindings, so we strip trailing requires +# clauses before parsing. The pattern is anchored to a function signature `) [const] [noexcept]` +# (and stops at the body `{` or `;`) so it never touches the word "requires" in comments or strings. +_TRAILING_REQUIRES = re.compile(r"(\)\s*(?:const\s*)?(?:noexcept\s*)?)requires\b[^{};]*?(?=[{;])") + + +def _code_preprocess(code: str) -> str: + return _TRAILING_REQUIRES.sub(r"\1", code) + + +# //////////////////////////////////////////////////////////////////////////////////////////////// +# Post-processing of the generated pybind code +# //////////////////////////////////////////////////////////////////////////////////////////////// +# litgen/srcmlcpp have two bugs we cannot fix via options (see lib/README): +# * srcML inner-struct bug (srcML #2033): classes with a nested Config/etc. struct get a bogus +# `py::init<>() // implicit default constructor`, even though they are not default-constructible. +# * Nested structs of *template* classes (RangeMapper::Config, Bezier::Config) and the +# pyClass variable references for them are emitted without the template argument. +# These used to be fixed by hand after every regeneration. _postprocess_generated() reapplies them +# automatically so the generated file compiles as-is. + +# Map of "main class" pyClass chain-head variable -> replacement for its bogus implicit default +# constructor. An empty string removes the constructor entirely (class is not constructible here). +_IMPLICIT_CTOR_FIX = { + "pyClassLogger": ".def(py::init())", + "pyClassGaussian": ".def(py::init())", + "pyClassPid": ".def(py::init())", + "pyClassTcpSocket": ".def(py::init())", + "pyClassUdpSocket": ".def(py::init())", + "pyClassJoystick": ".def(py::init())", + "pyClassTimer": ( + ".def(py::init())\n" + " .def(py::init())" + ), + "pyClassBezier_espp_Vector2f": ( + ".def(py::init::Config &>())\n" + " .def(py::init::WeightedConfig &>())" + ), + "pyClassTask": ( + ".def(py::init())\n" + " .def(py::init())" + ), + "pyClassSocket": "", # base class: drop the bogus default constructor entirely + # RTSP: abstract base packetizers must not be constructible; the concrete ones take a Config. + "pyClassRtpDepacketizer": "", # abstract base + "pyClassRtpPacketizer": "", # abstract base + "pyClassGenericDepacketizer": ".def(py::init())", + "pyClassGenericPacketizer": ".def(py::init())", + "pyClassH264Depacketizer": ".def(py::init())", + "pyClassH264Packetizer": ".def(py::init())", + "pyClassMjpegDepacketizer": ".def(py::init())", + "pyClassMjpegPacketizer": ".def(py::init())", + "pyClassRtspClient": ".def(py::init())", + "pyClassRtspServer": ".def(py::init())", + "pyClassRtspSession": ( + ".def(py::init, const espp::RtspSession::Config &>())" + ), +} + + +def _fix_implicit_default_ctors(code: str) -> str: + for chain_head, replacement in _IMPLICIT_CTOR_FIX.items(): + pattern = re.compile( + r"\n " + re.escape(chain_head) + r"\n" + r" \.def\(py::init<>\(\)\) // implicit default constructor\n" + ) + new_line = (f"\n {chain_head}\n {replacement}\n") if replacement else f"\n {chain_head}\n" + code, n = pattern.subn(lambda m, nl=new_line: nl, code) + if n != 1: + print(f"WARNING: implicit-ctor fix for {chain_head} applied {n} times (expected 1)") + return code + + +def _fix_template_class_nested(code: str) -> str: + # Bezier: single specialization (espp::Vector2f). The outer class is already emitted correctly, + # but its nested Config/WeightedConfig and the bare `pyClassBezier` references are not. These + # patterns occur only in the Bezier section, so a global replace is safe. `(?!\w)` avoids + # touching the correctly-suffixed variables (pyClassBezier_espp_Vector2f, *_ClassConfig). + code = code.replace("espp::Bezier::", "espp::Bezier::") + code = re.sub(r"pyClassBezier(?!\w)", "pyClassBezier_espp_Vector2f", code) + + # RangeMapper: two specializations (, ) share the raw `espp::RangeMapper::` text and + # bare `pyClassRangeMapper`, so they must be fixed per-block. + code = _fix_specialized_block( + code, "pyClassRangeMapper_int", "pyClassRangeMapper_float", + "espp::RangeMapper", "int", "pyClassRangeMapper", + ) + code = _fix_specialized_block( + code, "pyClassRangeMapper_float", None, + "espp::RangeMapper", "float", "pyClassRangeMapper", + ) + + # Vector2d: two specializations (, ), same nested/bare-reference issue as RangeMapper. + code = _fix_specialized_block( + code, "pyClassVector2d_int", "pyClassVector2d_float", + "espp::Vector2d", "int", "pyClassVector2d", + ) + code = _fix_specialized_block( + code, "pyClassVector2d_float", None, + "espp::Vector2d", "float", "pyClassVector2d", + ) + return code + + +def _fix_specialized_block(code, start_var, next_var, raw_type, targ, bare_pyclass): + start = code.find(f"auto {start_var} =") + if start < 0: + return code + if next_var is not None: + end = code.find(f"auto {next_var} =", start) + else: + # end of this specialization's block: the next top-level ` auto pyClass...` declaration. + m = re.search(r"\n auto pyClass", code[start + 1:]) + end = (start + 1 + m.start()) if m else len(code) + if end < 0: + end = len(code) + block = code[start:end] + # Add the template argument to every bare use of the class (both `raw_type::Member` and + # bare-type uses like `const raw_type &` / `py::class_`). `(?!<)` skips uses that are + # already specialized (e.g. the `py::class_>` header itself). + block = re.sub(re.escape(raw_type) + r"(?!<)", f"{raw_type}<{targ}>", block) + block = re.sub(re.escape(bare_pyclass) + r"(?!\w)", f"{start_var}", block) + return code[:start] + block + code[end:] + + +# Methods that exist as BOTH an instance and a static overload of the same name (e.g. +# `Task::get_id()` and `static Task::get_id(const Task&)`). litgen binds both, but pybind11 refuses +# to register a static and instance method under one name (fails at import). Drop the static one. +_STATIC_INSTANCE_DUP_METHODS = ["get_id"] + + +def _remove_static_instance_dups(code: str) -> str: + for name in _STATIC_INSTANCE_DUP_METHODS: + # Remove the `.def_static("name", ... )` block up to (not including) the next binding. + code = re.sub( + r"\n \.def_static\(\"" + re.escape(name) + r"\",.*?(?=\n \.def)", + "", + code, + flags=re.DOTALL, + ) + return code + + +# litgen drops the base class and std::shared_ptr holder for the RTP packetizer hierarchy and +# JpegFrame. These are returned/stored as std::shared_ptr (callbacks, RtpDepacketizer factories), so +# pybind needs the shared_ptr holder, and the inheritance so derived/base convert. Map class name -> +# full `py::class_<...>` template arguments (base classes must precede the holder, and a base must be +# registered before its derived class, which matches the header/generation order). +_CLASS_HOLDER_FIX = { + "JpegFrame": "espp::JpegFrame, std::shared_ptr", + "RtpDepacketizer": "espp::RtpDepacketizer, std::shared_ptr", + "RtpPacketizer": "espp::RtpPacketizer, std::shared_ptr", + "GenericDepacketizer": + "espp::GenericDepacketizer, espp::RtpDepacketizer, std::shared_ptr", + "GenericPacketizer": + "espp::GenericPacketizer, espp::RtpPacketizer, std::shared_ptr", + "H264Depacketizer": + "espp::H264Depacketizer, espp::RtpDepacketizer, std::shared_ptr", + "H264Packetizer": + "espp::H264Packetizer, espp::RtpPacketizer, std::shared_ptr", + "MjpegDepacketizer": + "espp::MjpegDepacketizer, espp::RtpDepacketizer, std::shared_ptr", + "MjpegPacketizer": + "espp::MjpegPacketizer, espp::RtpPacketizer, std::shared_ptr", +} + + +def _fix_class_holders(code: str) -> str: + for name, args in _CLASS_HOLDER_FIX.items(): + # The trailing '>' ensures we match the bare `py::class_` and not + # `py::class_`. + bare = f"py::class_" + replacement = f"py::class_<{args}>" + if bare in code: + code = code.replace(bare, replacement) + else: + print(f"WARNING: class-holder fix for {name} did not match {bare}") + return code + + +def _postprocess_generated(code: str) -> str: + code = _fix_implicit_default_ctors(code) + code = _fix_template_class_nested(code) + code = _remove_static_instance_dups(code) + code = _fix_class_holders(code) + return code def my_litgen_options() -> litgen.LitgenOptions: @@ -40,6 +242,11 @@ def my_litgen_options() -> litgen.LitgenOptions: # We want to exclude `inline void priv_SetOptions(bool v) {}` from the bindings # priv_ is a prefix for private functions that we don't want to expose # options.fn_exclude_by_name__regex = "run_on_core" # NOTE: this doesn't work since it seems to be parsing that fails for Task::run_on_core + # Exclude Vector2d's floating-point-only members. They are guarded by a `requires` clause that + # we strip at parse time (see _code_preprocess) so the class can be parsed; without the clause + # litgen would otherwise bind them for Vector2d too, which does not compile. These names are + # unique to Vector2d, so excluding them globally is safe. + options.fn_exclude_by_name__regex = r"^(inv_magnitude|angle|signed_angle|rotated)$" # we'd like the following classes to be able to pick up new attributes # dynamically (within python): @@ -96,6 +303,13 @@ def my_litgen_options() -> litgen.LitgenOptions: # Set to True if you want the stub file to be formatted with black options.python_run_black_formatter = False + # //////////////////////////////////////////////////////////////////// + # Parse-time preprocessing (strip trailing C++20 requires-clauses) + # //////////////////////////////////////////////////////////////////// + # Without this, srcmlcpp mis-parses Vector2d (its members use trailing requires-clauses) and the + # `Vector2d` template specializations above silently fail to generate. See _code_preprocess. + options.srcmlcpp_options.code_preprocess_function = _code_preprocess + return options @@ -105,6 +319,7 @@ def autogenerate() -> None: include_dir = repository_dir + "/components/" header_files = [include_dir + "base_component/include/base_component.hpp", + # cdr.hpp is bound by hand in python_bindings/cdr_bindings.cpp (see note above). include_dir + "cobs/include/cobs.hpp", include_dir + "cobs/include/cobs_stream.hpp", include_dir + "color/include/color.hpp", @@ -129,6 +344,13 @@ def autogenerate() -> None: # NOTE: this must come after vector2d.hpp and range_mapper.hpp since it depends on them! include_dir + "joystick/include/joystick.hpp", + # NOTE: RtpsParticipant has many nested types (GuidPrefix, EntityId, + # Locator::Kind, ...), std::function callbacks, and std::span APIs that litgen + # does not bind cleanly (it emits unqualified nested type names that do not + # compile). Like cdr, it is best exposed via a hand-written shim; deferred for + # now so the rest of the module generates and compiles. + # include_dir + "rtps/include/rtps.hpp", + # NOTE: this must come after socket since it depends on it! include_dir + "rtsp/include/rtp_types.hpp", include_dir + "rtsp/include/rtp_depacketizer.hpp", @@ -174,13 +396,40 @@ def autogenerate() -> None: # include_dir + "serialization/include/serialization.hpp", ] + pydef_file = output_dir + "/pybind_espp.cpp" litgen.write_generated_code_for_files( options=my_litgen_options(), input_cpp_header_files=header_files, - output_cpp_pydef_file=output_dir + "/pybind_espp.cpp", + output_cpp_pydef_file=pydef_file, output_stub_pyi_file=output_dir + "/espp/__init__.pyi", ) + # Reapply the fixes that litgen/srcmlcpp cannot do via options, so the generated file compiles + # without any manual editing (see _postprocess_generated). + with open(pydef_file, "r") as f: + code = f.read() + code = _postprocess_generated(code) + with open(pydef_file, "w") as f: + f.write(code) + print(f"Post-processed {pydef_file}") + + # Apply the compiler-driven nested-scope qualification fixes (RTSP/rtps), so the generated file + # compiles with zero manual edits. Requires a configured build (compile_commands.json); if that + # is missing, skip with a hint rather than failing. + try: + import fix_generated_bindings + if os.path.exists(fix_generated_bindings.COMPILE_DB): + print("Applying compiler-driven qualification fixes...") + fix_generated_bindings.main() + else: + print( + "Skipping qualification fixer: no build/compile_commands.json. Configure once with\n" + " cmake -DCMAKE_EXPORT_COMPILE_COMMANDS=ON .. (from lib/build), then run\n" + " python fix_generated_bindings.py" + ) + except Exception as exc: # noqa: BLE001 - generation should not hard-fail on the optional fixer + print(f"Qualification fixer did not run ({exc}); run python fix_generated_bindings.py manually.") + if __name__ == "__main__": autogenerate() diff --git a/lib/espp.cmake b/lib/espp.cmake index e630ed5ea..12432b4fd 100644 --- a/lib/espp.cmake +++ b/lib/espp.cmake @@ -97,6 +97,8 @@ endif() set(ESPP_PYTHON_SOURCES ${CMAKE_CURRENT_LIST_DIR}/python_bindings/module.cpp ${CMAKE_CURRENT_LIST_DIR}/python_bindings/pybind_espp.cpp + ${CMAKE_CURRENT_LIST_DIR}/python_bindings/cdr_bindings.cpp + ${CMAKE_CURRENT_LIST_DIR}/python_bindings/rtps_bindings.cpp ${ESPP_SOURCES} ) diff --git a/lib/fix_generated_bindings.py b/lib/fix_generated_bindings.py new file mode 100644 index 000000000..5a5c89254 --- /dev/null +++ b/lib/fix_generated_bindings.py @@ -0,0 +1,154 @@ +#!/usr/bin/env python3 +"""Apply clang's nested-scope qualification fixes to the generated pybind file. + +litgen 0.22 emits *unqualified* nested-type / nested-static names across classes that have nested +types (e.g. `frame_callback_t` instead of `espp::RtspClient::frame_callback_t`, `GuidPrefix` instead +of `espp::RtpsParticipant::GuidPrefix`, `Kind` instead of `espp::RtpsParticipant::Locator::Kind`). +These were previously fixed by hand after every regeneration (the RTSP/rtps ones were never even +documented). + +clang reports each one with a precise `... 'Y'; did you mean 'espp::X::Y'?` suggestion. This script +compiles the generated file (syntax-only, all errors at once), applies those suggestions +positionally, and repeats until the file compiles cleanly. Run it after autogenerate_bindings.py +(which autogenerate invokes automatically). +""" +from __future__ import annotations + +import json +import os +import re +import shlex +import subprocess +import sys + +HERE = os.path.dirname(os.path.realpath(__file__)) +PYDEF = os.path.join(HERE, "python_bindings", "pybind_espp.cpp") +# cmake writes the exact compile command (with every include dir / define) here. Generate it with: +# cmake -DCMAKE_EXPORT_COMPILE_COMMANDS=ON .. (from lib/build) +COMPILE_DB = os.path.join(HERE, "build", "compile_commands.json") + + +def _compile_command() -> list[str]: + if not os.path.exists(COMPILE_DB): + sys.exit( + f"{COMPILE_DB} not found. Configure the build once with\n" + f" cmake -DCMAKE_EXPORT_COMPILE_COMMANDS=ON ..\n" + f"from lib/build so the exact compile flags are available." + ) + db = json.load(open(COMPILE_DB)) + entry = next(c for c in db if c["file"].endswith("pybind_espp.cpp")) + argv = shlex.split(entry["command"]) + # Drop output (-o x.o) and the input file; add syntax-only + all-errors. + out: list[str] = [] + skip = False + for tok in argv: + if skip: + skip = False + continue + if tok == "-o": + skip = True + continue + if tok.endswith("pybind_espp.cpp.o") or tok.endswith("pybind_espp.cpp") or tok == "-c": + continue + out.append(tok) + out += ["-fsyntax-only", "-ferror-limit=0", PYDEF] + return out + + +def _compile_errors() -> str: + result = subprocess.run(_compile_command(), capture_output=True, text=True) + return result.stderr + + +_SUGGEST_RE = re.compile( + r"pybind_espp\.cpp:(\d+):(\d+): error: [^\n]*?'(\w+)'; did you mean '([^']+)'\?" +) +_UNDECL_RE = re.compile( + r"pybind_espp\.cpp:(\d+):(\d+): error: (?:use of undeclared identifier|unknown type name) '(\w+)'" +) +# A def_readwrite on a non-copyable member (e.g. a UdpSocket field) fails to compile; the note +# points back at the offending `.def_readwrite(...)` line, which must become `.def_readonly(...)`. +_READWRITE_NOTE_RE = re.compile( + r"pybind_espp\.cpp:(\d+):\d+: note: in instantiation of .*def_readwrite.* requested here" +) + + +_ENCLOSING_CLASS_RE = re.compile(r"py::class_ str | None: + """Find the espp class whose binding block contains line_index (nearest preceding py::class_).""" + for i in range(line_index, -1, -1): + m = _ENCLOSING_CLASS_RE.search(lines[i]) + if m: + return m.group(1) + return None + + +def _fix_once() -> tuple[int, int]: + """Apply one round of fixes. Returns (remaining_error_count, edits_applied).""" + err = _compile_errors() + error_count = err.count(": error:") + lines = open(PYDEF).read().split("\n") + + # Learn the bare->qualified map from clang's suggestions (only accept espp:: scope suggestions + # that genuinely qualify the same identifier, e.g. Y -> espp::X::Y; skip noise like + # "did you mean 'bind'?"). + qualified: dict[str, str] = {} + edits: list[tuple[int, int, str, str]] = [] + for m in _SUGGEST_RE.finditer(err): + ln, col, bare, qual = int(m.group(1)), int(m.group(2)), m.group(3), m.group(4) + if qual.startswith("espp::") and qual.endswith("::" + bare): + qualified[bare] = qual + edits.append((ln, col, bare, qual)) + # Undeclared/unknown identifiers without a (usable) suggestion: reuse the learned mapping, or + # fall back to the enclosing class. Some nested typedefs (e.g. frame_callback_t, defined in both + # RtpDepacketizer and RtspClient) get no clang suggestion and are ambiguous, so we qualify them + # with the class whose binding block contains the error: `espp::::`. + for m in _UNDECL_RE.finditer(err): + ln, col, bare = int(m.group(1)), int(m.group(2)), m.group(3) + if bare in qualified: + edits.append((ln, col, bare, qualified[bare])) + else: + enclosing = _enclosing_class(lines, ln - 1) + if enclosing is not None: + edits.append((ln, col, bare, f"espp::{enclosing}::{bare}")) + + # Apply positionally, rightmost-first within a line so earlier columns don't shift. + applied = 0 + for ln, col, bare, qual in sorted(set(edits), key=lambda e: (e[0], e[1]), reverse=True): + i, c = ln - 1, col - 1 + if 0 <= i < len(lines) and lines[i][c:c + len(bare)] == bare: + lines[i] = lines[i][:c] + qual + lines[i][c + len(bare):] + applied += 1 + + # Demote def_readwrite -> def_readonly for non-copyable members the compiler flagged. + for m in _READWRITE_NOTE_RE.finditer(err): + i = int(m.group(1)) - 1 + if 0 <= i < len(lines) and "def_readwrite" in lines[i]: + lines[i] = lines[i].replace("def_readwrite", "def_readonly", 1) + applied += 1 + + if applied: + open(PYDEF, "w").write("\n".join(lines)) + return error_count, applied + + +def main() -> int: + for iteration in range(60): + error_count, applied = _fix_once() + print(f" iter {iteration}: errors={error_count} qualifications_applied={applied}") + if error_count == 0: + print("Generated bindings compile cleanly.") + return 0 + if applied == 0: + print("No more qualification fixes to apply; remaining errors are not scope issues:") + remaining = [l for l in _compile_errors().splitlines() if ": error:" in l] + print("\n".join(remaining[:25])) + return 1 + print("Did not converge within iteration limit.") + return 1 + + +if __name__ == "__main__": + sys.exit(main()) diff --git a/lib/python_bindings/cdr_bindings.cpp b/lib/python_bindings/cdr_bindings.cpp new file mode 100644 index 000000000..ebdee5aac --- /dev/null +++ b/lib/python_bindings/cdr_bindings.cpp @@ -0,0 +1,246 @@ +// Hand-written pybind11 bindings for the `cdr` component. +// +// Why hand-written instead of litgen-generated: +// - litgen/srcmlcpp cannot parse cdr.hpp (its class parser chokes on the method bodies / +// requires-clauses), and even when coerced it produces an unusable API: +// * CdrReader::read(T& value) is an *output* reference, which pybind cannot return to +// Python (the decoded value is lost). +// * CdrReader holds a NON-OWNING std::span, so a generated `CdrReader(bytes)` would dangle +// once the Python buffer is freed. +// - This shim exposes a clean, safe, Pythonic API: write_*(value), read_*() -> Optional[...], +// bytes in/out, and an owning reader wrapper that copies its input buffer. +// +// It is intentionally separate from the generated pybind_espp.cpp so regeneration never clobbers +// it, and it is easy to extend with new CDR helpers. + +#include +#include +#include +#include +#include + +#include +#include + +#include "cdr.hpp" + +namespace py = pybind11; + +namespace { + +std::vector bytes_to_vec(const py::bytes &data) { + // py::bytes -> std::string -> bytes copy (owning). + std::string s = data; + return std::vector(s.begin(), s.end()); +} + +py::bytes span_to_bytes(std::span s) { + return py::bytes(reinterpret_cast(s.data()), s.size()); +} + +py::bytes vec_to_bytes(const std::vector &v) { + return py::bytes(reinterpret_cast(v.data()), v.size()); +} + +// Owning wrapper around CdrReader: CdrReader stores a non-owning std::span, so we keep the decoded +// buffer alive for the lifetime of the reader. Declaration order matters: `storage_` must be +// initialized before `reader_` (which references it). +class PyCdrReader { +public: + explicit PyCdrReader(const py::bytes &data, + espp::CdrReader::Config config = espp::CdrReader::Config{}) + : storage_(bytes_to_vec(data)) + , reader_(std::span{storage_.data(), storage_.size()}, config) {} + + static PyCdrReader + make_body_reader(const py::bytes &data, + espp::CdrEncapsulation encapsulation = espp::CdrEncapsulation::CDR_LE) { + return PyCdrReader(data, espp::CdrReader::body_config(encapsulation)); + } + + bool valid() const { return reader_.valid(); } + espp::CdrEncapsulation encapsulation() const { return reader_.encapsulation(); } + bool uses_little_endian() const { return reader_.uses_little_endian(); } + size_t remaining() const { return reader_.remaining(); } + py::bytes payload() const { return span_to_bytes(reader_.payload()); } + py::bytes remaining_view() const { return span_to_bytes(reader_.remaining_view()); } + bool skip(size_t length) { return reader_.skip(length); } + bool align(size_t alignment) { return reader_.align(alignment); } + + template std::optional read() { + T value{}; + if (!reader_.read(value)) { + return std::nullopt; + } + return value; + } + + std::optional read_bool() { + bool value = false; + if (!reader_.read_bool(value)) { + return std::nullopt; + } + return value; + } + + std::optional read_string() { + std::string value; + if (!reader_.read_string(value)) { + return std::nullopt; + } + return value; + } + + std::optional read_bytes(size_t length, size_t alignment = 1) { + std::vector bytes; + if (!reader_.read_bytes(bytes, length, alignment)) { + return std::nullopt; + } + return vec_to_bytes(bytes); + } + + template std::optional> read_sequence() { + std::vector values; + if (!reader_.read_sequence(values)) { + return std::nullopt; + } + return values; + } + +private: + std::vector storage_; + espp::CdrReader reader_; +}; + +template void add_writer_scalar(py::class_ &c, const char *name) { + c.def( + name, [](espp::CdrWriter &w, T value) { return w.write(value); }, py::arg("value")); +} + +template void add_writer_sequence(py::class_ &c, const char *name) { + c.def( + name, + [](espp::CdrWriter &w, const std::vector &values) { + return w.write_sequence(std::span{values.data(), values.size()}); + }, + py::arg("values")); +} + +template void add_reader_scalar(py::class_ &c, const char *name) { + c.def(name, &PyCdrReader::read); +} + +template void add_reader_sequence(py::class_ &c, const char *name) { + c.def(name, &PyCdrReader::read_sequence); +} + +} // namespace + +void py_init_cdr(py::module &m) { + py::enum_(m, "CdrEncapsulation", + "Supported CDR encapsulation identifiers.") + .value("CDR_BE", espp::CdrEncapsulation::CDR_BE) + .value("CDR_LE", espp::CdrEncapsulation::CDR_LE) + .value("PL_CDR_BE", espp::CdrEncapsulation::PL_CDR_BE) + .value("PL_CDR_LE", espp::CdrEncapsulation::PL_CDR_LE); + + // ---- CdrWriter (owns its buffer, so it is safe to bind directly) ---- + auto writer = py::class_(m, "CdrWriter", py::dynamic_attr(), + "Helper for building CDR/XCDR1-style byte streams."); + + py::class_(writer, "Config") + .def(py::init<>()) + .def_readwrite("encapsulation", &espp::CdrWriter::Config::encapsulation) + .def_readwrite("include_encapsulation", &espp::CdrWriter::Config::include_encapsulation); + + writer.def(py::init<>()) + .def(py::init(), py::arg("config")) + .def_static("make_body_writer", &espp::CdrWriter::make_body_writer, + py::arg("encapsulation") = espp::CdrEncapsulation::CDR_LE) + .def_static( + "encapsulate", + [](const py::bytes &payload, espp::CdrEncapsulation encapsulation) { + auto vec = bytes_to_vec(payload); + return vec_to_bytes(espp::CdrWriter::encapsulate( + std::span{vec.data(), vec.size()}, encapsulation)); + }, + py::arg("payload"), py::arg("encapsulation") = espp::CdrEncapsulation::CDR_LE) + .def("reset", &espp::CdrWriter::reset) + .def("encapsulation", &espp::CdrWriter::encapsulation) + .def("uses_little_endian", &espp::CdrWriter::uses_little_endian) + .def("size", &espp::CdrWriter::size) + .def("align", &espp::CdrWriter::align, py::arg("alignment")) + .def("write_bool", &espp::CdrWriter::write_bool, py::arg("value")) + .def("write_string", &espp::CdrWriter::write_string, py::arg("text")) + .def( + "write_bytes", + [](espp::CdrWriter &w, const py::bytes &data, size_t alignment) { + auto vec = bytes_to_vec(data); + return w.write_bytes(std::span{vec.data(), vec.size()}, alignment); + }, + py::arg("data"), py::arg("alignment") = 1) + .def("buffer", [](const espp::CdrWriter &w) { return vec_to_bytes(w.buffer()); }) + .def("payload", [](const espp::CdrWriter &w) { return span_to_bytes(w.payload()); }) + .def("take_buffer", [](espp::CdrWriter &w) { return vec_to_bytes(w.take_buffer()); }); + + add_writer_scalar(writer, "write_uint8"); + add_writer_scalar(writer, "write_uint16"); + add_writer_scalar(writer, "write_uint32"); + add_writer_scalar(writer, "write_uint64"); + add_writer_scalar(writer, "write_int8"); + add_writer_scalar(writer, "write_int16"); + add_writer_scalar(writer, "write_int32"); + add_writer_scalar(writer, "write_int64"); + add_writer_scalar(writer, "write_float"); + add_writer_scalar(writer, "write_double"); + add_writer_sequence(writer, "write_sequence_uint8"); + add_writer_sequence(writer, "write_sequence_uint16"); + add_writer_sequence(writer, "write_sequence_uint32"); + add_writer_sequence(writer, "write_sequence_int32"); + add_writer_sequence(writer, "write_sequence_float"); + add_writer_sequence(writer, "write_sequence_double"); + + // ---- CdrReader (owning wrapper; read_* return Optional, None on failure) ---- + auto reader = py::class_(m, "CdrReader", py::dynamic_attr(), + "Helper for parsing CDR/XCDR1-style byte streams. Copies " + "the input buffer so it is safe to use independently."); + + py::class_(reader, "Config") + .def(py::init<>()) + .def_readwrite("expect_encapsulation", &espp::CdrReader::Config::expect_encapsulation) + .def_readwrite("default_encapsulation", &espp::CdrReader::Config::default_encapsulation); + + reader + .def(py::init(), py::arg("data"), + py::arg("config") = espp::CdrReader::Config{}) + .def_static("make_body_reader", &PyCdrReader::make_body_reader, py::arg("data"), + py::arg("encapsulation") = espp::CdrEncapsulation::CDR_LE) + .def("valid", &PyCdrReader::valid) + .def("encapsulation", &PyCdrReader::encapsulation) + .def("uses_little_endian", &PyCdrReader::uses_little_endian) + .def("remaining", &PyCdrReader::remaining) + .def("payload", &PyCdrReader::payload) + .def("remaining_view", &PyCdrReader::remaining_view) + .def("skip", &PyCdrReader::skip, py::arg("length")) + .def("align", &PyCdrReader::align, py::arg("alignment")) + .def("read_bool", &PyCdrReader::read_bool) + .def("read_string", &PyCdrReader::read_string) + .def("read_bytes", &PyCdrReader::read_bytes, py::arg("length"), py::arg("alignment") = 1); + + add_reader_scalar(reader, "read_uint8"); + add_reader_scalar(reader, "read_uint16"); + add_reader_scalar(reader, "read_uint32"); + add_reader_scalar(reader, "read_uint64"); + add_reader_scalar(reader, "read_int8"); + add_reader_scalar(reader, "read_int16"); + add_reader_scalar(reader, "read_int32"); + add_reader_scalar(reader, "read_int64"); + add_reader_scalar(reader, "read_float"); + add_reader_scalar(reader, "read_double"); + add_reader_sequence(reader, "read_sequence_uint8"); + add_reader_sequence(reader, "read_sequence_uint16"); + add_reader_sequence(reader, "read_sequence_uint32"); + add_reader_sequence(reader, "read_sequence_int32"); + add_reader_sequence(reader, "read_sequence_float"); + add_reader_sequence(reader, "read_sequence_double"); +} diff --git a/lib/python_bindings/espp/__init__.pyi b/lib/python_bindings/espp/__init__.pyi index 4bb845263..89816b2bc 100644 --- a/lib/python_bindings/espp/__init__.pyi +++ b/lib/python_bindings/espp/__init__.pyi @@ -87,7 +87,8 @@ class Cobs: * * Provides single-packet encoding and decoding using the COBS algorithm * with 0 as the delimiter. - * COBS encoding can add at most ⌈n/254⌉ + 1 bytes overhead. Plus 1 byte for the delimiter + * COBS encoding can add at most ceil(n/254) + 1 bytes overhead, plus 1 byte + * for the delimiter. * COBS changes the size of the packet by at least 1 byte, so it's not possible to encode in * place. MAX_BLOCK_SIZE = 254 is the maximum number of non-zero bytes in an encoded block. * @@ -338,9 +339,9 @@ class Rgb: * @brief Class representing a color using RGB color space. """ - r: float = float(0) #/< Red value ∈ [0, 1] - g: float = float(0) #/< Green value ∈ [0, 1] - b: float = float(0) #/< Blue value ∈ [0, 1] + r: float = float(0) #/< Red value in [0, 1] + g: float = float(0) #/< Green value in [0, 1] + b: float = float(0) #/< Blue value in [0, 1] @overload def __init__(self) -> None: @@ -443,9 +444,9 @@ class Hsv: * @brief Class representing a color using HSV color space. """ - h: float = float(0) #/< Hue ∈ [0, 360] - s: float = float(0) #/< Saturation ∈ [0, 1] - v: float = float(0) #/< Value ∈ [0, 1] + h: float = float(0) #/< Hue in [0, 360] + s: float = float(0) #/< Saturation in [0, 1] + v: float = float(0) #/< Value in [0, 1] @overload def __init__(self) -> None: @@ -1618,832 +1619,1368 @@ class RangeMapper_float: # Python specialization for RangeMapper +# ------------------------------------------------------------------------ +# +# ------------------------------------------------------------------------ - def __init__(self) -> None: - """Auto-generated default constructor""" - pass -#################### #################### -#################### #################### +#################### #################### +#################### #################### -class TcpSocket: +class Ndef: """* - * @brief Class for managing sending and receiving data using TCP/IP. Can be - * used to create client or server sockets. + * @brief implements serialization & deserialization logic for NFC Data + * Exchange Format (NDEF) records which can be stored on and + * transmitted from NFC devices. * - * \section tcp_ex1 TCP Client Example - * \snippet socket_example.cpp TCP Client example - * \section tcp_ex2 TCP Server Example - * \snippet socket_example.cpp TCP Server example + * @details NDEF records can be composed the following way: + * @code{.unparsed} + * Bit 7 6 5 4 3 2 1 0 + * ------ ------ ------ ------ ------ ------ ------ ------ + * [ MB ] [ ME ] [ CF ] [ SR ] [ IL ] [ TNF ] + * [ TYPE LENGTH (may be 0) ] + * [ PAYLOAD LENGTH (1B or 4B, see SR) ] + * [ ID LENGTH (if IL) ] + * [ RECORD TYPE (if TYPE LENGTH > 0) ] + * [ ID (if IL) ] + * [ PAYLOAD (payload length bytes)] + * @endcode * - * \section tcp_ex3 TCP Client Response Example - * \snippet socket_example.cpp TCP Client Response example - * \section tcp_ex4 TCP Server Response Example - * \snippet socket_example.cpp TCP Server Response example + * The first byte (Flags) has these bits: + * * Bits 0-3: TNF - Type Name Format - describes record type (see TNF class) + * * Bit 3: IL - ID Length - indicates if the ID Length Field is present or not + * * Bit 4: SR - Short Record - set to 1 if the payload length field is 1 byte (8 + * bits / 0-255) or less, otherwise the payload length is 4 bytes + * * Bit 5: CF - Chunk Flag - indicates if this is the first record chunk or a + * middle record chunk, set to 0 for the first record of the message and + * for subsequent records set to 1. + * * Bit 6: ME - Message End - 1 indicates if this is the last record in the + * message + * * Bit 7: MB - Message Begin - 1 indicates if this is the first record in the + * message + * + * @note Some information about NDEF can be found: + * * https://www.maskaravivek.com/post/understanding-the-format-of-ndef-messages/ + * * https://ndeflib.readthedocs.io/en/stable/records/bluetooth.html + * * https://developer.android.com/reference/android/nfc/NdefMessage + * * https://www.oreilly.com/library/view/beginning-nfc/9781449324094/ch04.html + * * https://learn.adafruit.com/adafruit-pn532-rfid-nfc/ndef * """ - class Config: + class TNF(enum.IntEnum): """* - * @brief Config struct for the TCP socket. + * @brief Type Name Format (TNF) field is a 3-bit value that describes the + * record type. + * + * Some Common TNF::WELL_KNOWN record type strings: + * * Text (T) + * * URI (U) + * * Smart Poster (Sp) + * * Alternative Carrier (ac) + * * Handover Carrier (Hc) + * * Handover Request (Hr) + * * Handover Select (Hs) """ - log_level: Logger.Verbosity = Logger.Verbosity(Logger.Verbosity.warn) #*< Verbosity level for the TCP socket logger. - def __init__( - self, - log_level: Logger.Verbosity = Logger.Verbosity(Logger.Verbosity.warn) - ) -> None: - """Auto-generated default constructor with named params""" - pass + empty = enum.auto() # (= 0x00) #/< Record is empty + well_known = enum.auto() # (= 0x01) #/< Type field contains a well-known RTD type name + mime_media = enum.auto() # (= 0x02) #/< Type field contains a media type (RFC 2046) + absolute_uri = enum.auto() # (= 0x03) #/< Type field contains an absolute URI (RFC 3986) + external_type = enum.auto() # (= 0x04) #/< Type field Contains an external type name + unknown = enum.auto() # (= 0x05) #/< Payload type is unknown, type length must be 0. + unchanged = enum.auto() # (= 0x06) #/< Indicates the payload is an intermediate or final chunk of a chunked NDEF + #/< record, type length must be 0. + reserved = enum.auto() # (= 0x07) #/< Reserved by the NFC forum for future use - class ConnectConfig: + class Uic(enum.IntEnum): """* - * @brief Config struct for connecting to a remote TCP server. + * URI Identifier Codes (UIC), See Table A-3 at + * https://www.oreilly.com/library/view/beginning-nfc/9781449324094/apa.html + * and https://learn.adafruit.com/adafruit-pn532-rfid-nfc/ndef """ - ip_address: str #*< Address to send data to. - port: int #*< Port number to send data to. - def __init__(self, ip_address: str = "", port: int = int()) -> None: - """Auto-generated default constructor with named params""" - pass - - class TransmitConfig: + none = enum.auto() # (= 0x00) #/< Exactly as written + http_www = enum.auto() # (= 0x01) #/< http://www. + https_www = enum.auto() # (= 0x02) #/< https://www. + http = enum.auto() # (= 0x03) #/< http:// + https = enum.auto() # (= 0x04) #/< https:// + tel = enum.auto() # (= 0x05) #/< tel: + mailto = enum.auto() # (= 0x06) #/< mailto: + ftp_anon = enum.auto() # (= 0x07) #/< ftp://anonymous:anonymous@ + ftp_ftp = enum.auto() # (= 0x08) #/< ftp://ftp. + ftps = enum.auto() # (= 0x09) #/< ftps:// + sftp = enum.auto() # (= 0x0A) #/< sftp:// + smb = enum.auto() # (= 0x0B) #/< smb:// + nfs = enum.auto() # (= 0x0C) #/< nfs:// + ftp = enum.auto() # (= 0x0D) #/< ftp:// + dav = enum.auto() # (= 0x0E) #/< dav:// + news = enum.auto() # (= 0x0F) #/< news: + telnet = enum.auto() # (= 0x10) #/< telnet:// + imap = enum.auto() # (= 0x11) #/< imap: + rstp = enum.auto() # (= 0x12) #/< rtsp:// + urn = enum.auto() # (= 0x13) #/< urn: + pop = enum.auto() # (= 0x14) #/< pop: + sip = enum.auto() # (= 0x15) #/< sip: + sips = enum.auto() # (= 0x16) #/< sips: + tftp = enum.auto() # (= 0x17) #/< tftp: + btspp = enum.auto() # (= 0x18) #/< btspp:// + btl2_cap = enum.auto() # (= 0x19) #/< btl2cap:// + btgoep = enum.auto() # (= 0x1A) #/< btgoep:// + tcpobex = enum.auto() # (= 0x1B) #/< tcpobex:// + irdaobex = enum.auto() # (= 0x1C) #/< irdaobex:// + file = enum.auto() # (= 0x1D) #/< file:// + urn_epc_id = enum.auto() # (= 0x1E) #/< urn:epc:id: + urn_epc_tag = enum.auto() # (= 0x1F) #/< urn:epc:tag: + urn_epc_pat = enum.auto() # (= 0x20) #/< urn:epc:pat: + urn_epc_raw = enum.auto() # (= 0x21) #/< urn:epc:raw: + urn_epc = enum.auto() # (= 0x22) #/< urn:epc: + urn_nfc = enum.auto() # (= 0x23) #/< urn:nfc: + + class BtType(enum.IntEnum): + """* + * @brief Type of Bluetooth radios. + + """ + bredr = enum.auto() # (= 0x00) #/< BT Classic + ble = enum.auto() # (= 0x01) #/< BT Low Energy + + class BtAppearance(enum.IntEnum): + """* + * @brief Some appearance codes for BLE radios. + + """ + unknown = enum.auto() # (= 0x0000) #/< Generic Unknown + # Generic Phone (b15-b6 = 0x001 << 6 = 0x0040) + phone = enum.auto() # (= 0x0040) #/< Generic Phone + # Generic Computer (b15-b6 = 0x002 << 6 = 0x0080) + computer = enum.auto() # (= 0x0080) #/< Generic Computer + # Generic Watch (b15-b6 = 0x003 << 6 = 0x00C0) + watch = enum.auto() # (= 0x00C0) #/< Generic Watch + # Generic Clock (b15-b6 = 0x004 << 6 = 0x0100) + clock = enum.auto() # (= 0x0100) #/< Generic Clock + # Generic Computer (b15-b6 = 0x005 << 6 = 0x0140) + display = enum.auto() # (= 0x0140) #/< Generic Display + # Generic Computer (b15-b6 = 0x006 << 6 = 0x0180) + remote_control = enum.auto() # (= 0x0180) #/< Generic Remote Control + # Generic HID (b15-b6 = 0x00F << 6 = 0x03C0) + generic_hid = enum.auto() # (= 0x03C0) #/< Generic HID + keyboard = enum.auto() # (= 0x03C1) #/< HID Keyboard + mouse = enum.auto() # (= 0x03C2) #/< HID Mouse + joystick = enum.auto() # (= 0x03C3) #/< HID Joystick + gamepad = enum.auto() # (= 0x03C4) #/< HID Gamepad + touchpad = enum.auto() # (= 0x03C9) #/< HID Touchpad + # Generic Gaming (b15-b6 = 0x02A << 6 = 0x0A80) + gaming = enum.auto() # (= 0x0A80) #/< Generic Gaming group + + class CarrierPowerState(enum.IntEnum): + """* + * @brief Power state of a BLE radio. + * @details Representation of the carrier power state in a Handover Select + * message. + + """ + inactive = enum.auto() # (= 0x00) #/< Carrier power is off + active = enum.auto() # (= 0x01) #/< Carrier power is on + activating = enum.auto() # (= 0x02) #/< Carrier power is turning on + unknown = enum.auto() # (= 0x03) #/< Carrier power state is unknown + + class BtEir(enum.IntEnum): + """* + * @brief Extended Inquiry Response (EIR) codes for data types in BT and BLE + * out of band (OOB) pairing NDEF records. + + """ + flags = enum.auto() # (= 0x01) #/< BT flags: b0: LE limited discoverable mode, b1: LE general discoverable mode, + #/< b2: BR/EDR not supported, b3: Simultaneous LE & BR/EDR controller, b4: + #/< simultaneous LE & BR/EDR Host + uuids_16_bit_partial = enum.auto() # (= 0x02) #/< Incomplete list of 16 bit service class UUIDs + uuids_16_bit_complete = enum.auto() # (= 0x03) #/< Complete list of 16 bit service class UUIDs + uuids_32_bit_partial = enum.auto() # (= 0x04) #/< Incomplete list of 32 bit service class UUIDs + uuids_32_bit_complete = enum.auto() # (= 0x05) #/< Complete list of 32 bit service class UUIDs + uuids_128_bit_partial = enum.auto() # (= 0x06) #/< Incomplete list of 128 bit service class UUIDs + uuids_128_bit_complete = enum.auto() # (= 0x07) #/< Complete list of 128 bit service class UUIDs + short_local_name = enum.auto() # (= 0x08) #/< Shortened Bluetooth Local Name + long_local_name = enum.auto() # (= 0x09) #/< Complete Bluetooth Local Name + tx_power_level = enum.auto() # (= 0x0A) #/< TX Power level (1 byte), -127 dBm to +127 dBm + class_of_device = enum.auto() # (= 0x0D) #/< Class of Device + sp_hash_c192 = enum.auto() # (= 0x0E) #/< Simple Pairing Hash C-192 + sp_random_r192 = enum.auto() # (= 0x0F) #/< Simple Pairing Randomizer R-192 + security_manager_tk = enum.auto() # (= 0x10) #/< Security Manager TK Value (LE Legacy Pairing) + security_manager_flags = enum.auto() # (= 0x11) #/< Flags (1 B), b0: OOB flags field (1 = 00B data present, 0 not), b1: LE Supported + #/< (host), b2: Simultaneous LE & BR/EDR to same device capable (host), b3: address + #/< type (0 = public, 1 = random) + appearance = enum.auto() # (= 0x19) #/< Appearance + mac = enum.auto() # (= 0x1B) #/< Bluetooth Device Address + le_role = enum.auto() # (= 0x1C) #/< LE Role + sp_hash_c256 = enum.auto() # (= 0x1D) #/< Simple Pairing Hash C-256 + sp_hash_r256 = enum.auto() # (= 0x1E) #/< Simple Pairing Randomizer R-256 + le_sc_confirmation = enum.auto() # (= 0x22) #/< LE Secure Connections Confirmation Value + le_sc_random = enum.auto() # (= 0x23) #/< LE Secure Connections Random Value + + class BleRole(enum.IntEnum): + """* + * @brief Possible roles for BLE records to indicate support for. + + """ + peripheral_only = enum.auto() # (= 0x00) #/< Radio can only act as a peripheral + central_only = enum.auto() # (= 0x01) #/< Radio can only act as a central + peripheral_central = enum.auto() # (= 0x02) #/< Radio can act as both a peripheral and a central, but prefers peripheral + central_peripheral = enum.auto() # (= 0x03) #/< Radio can act as both a peripheral and a central, but prefers central + + class WifiEncryptionType(enum.IntEnum): + """* + * @brief Types of configurable encryption for WiFi networks + + """ + none = enum.auto() # (= 0x01) #/< No encryption + wep = enum.auto() # (= 0x02) #/< WEP + tkip = enum.auto() # (= 0x04) #/< TKIP + aes = enum.auto() # (= 0x08) #/< AES + + class WifiAuthenticationType(enum.IntEnum): + """* + * @brief WiFi network authentication + + """ + open = enum.auto() # (= 0x01) #/< Open / no security + wpa_personal = enum.auto() # (= 0x02) #/< WPA personal + shared = enum.auto() # (= 0x04) #/< Shared key + wpa_enterprise = enum.auto() # (= 0x08) #/< WPA enterprise + wpa2_enterprise = enum.auto() # (= 0x10) #/< WPA2 Enterprise + wpa2_personal = enum.auto() # (= 0x20) #/< WPA2 personal + wpa_wpa2_personal = enum.auto() # (= 0x22) #/< Both WPA and WPA2 personal + + handover_version: int = 0x13 #/< Connection Handover version 1.3 # (C++ static member) # (const) + + def __init__( + self, + tnf: Ndef.TNF, + type: std.string_view, + payload: std.string_view + ) -> None: + """* + * @brief Makes an NDEF record with header and payload. + * @param tnf The TNF for this packet. + * @param type String view for the type of this packet + * @param payload The payload data for the packet + + """ + pass + + @staticmethod + def make_text(text: std.string_view) -> Ndef: + """* + * @brief Static function to make an NDEF record for transmitting english + * text. + * @param text The text that the NDEF record will hold. + * @return NDEF record object. + + """ + pass + + @staticmethod + def make_uri(uri: std.string_view, uic: Ndef.Uic = Ndef.Uic.none) -> Ndef: + """* + * @brief Static function to make an NDEF record for loading a URI. + * @param uri URI for the record to point to. + * @param uic UIC for the uri - helps shorten the uri text / NDEF record. + * @return NDEF record object. + + """ + pass + + @staticmethod + def make_android_launcher(uri: std.string_view) -> Ndef: + """* + * @brief Static function to make an NDEF record for launching an Android App. + * @param uri URI for the android package / app to launch. + * @return NDEF record object. + + """ + pass + + class WifiConfig: + """* + * @brief Configuration structure for wifi configuration ndef structure. + + """ + ssid: std.string_view #/< SSID for the network + key: std.string_view #/< Security key / password for the network + authentication: Ndef.WifiAuthenticationType = Ndef.WifiAuthenticationType.wpa2_personal #/< Authentication type the network + #/< uses. + encryption: Ndef.WifiEncryptionType = Ndef.WifiEncryptionType.aes #/< Encryption type the network uses. + mac_address: int = 0xFFFFFFFFFFFF #/< Broadcast MAC address FF:FF:FF:FF:FF:FF + def __init__( + self, + ssid: std.string_view = std.string_view(), + key: std.string_view = std.string_view(), + authentication: Ndef.WifiAuthenticationType = Ndef.WifiAuthenticationType.wpa2_personal, + encryption: Ndef.WifiEncryptionType = Ndef.WifiEncryptionType.aes, + mac_address: int = 0xFFFFFFFFFFFF + ) -> None: + """Auto-generated default constructor with named params""" + pass + + @staticmethod + def make_wifi_config(config: Ndef.WifiConfig) -> Ndef: + """* + * @brief Create a WiFi credential tag. + * @param config WifiConfig describing the WiFi network. + * @return NDEF record object. + + """ + pass + + @staticmethod + def make_collision_resolution_record(random_number: int) -> Ndef: + """ + * @brief Create a collision resolution record. + * @param random_number Random number to use for the collision resolution. + * @return NDEF record object. + + """ + pass + + @staticmethod + def make_handover_select(carrier_data_ref: int) -> Ndef: + """* + * @brief Create a Handover Select record for a Bluetooth device. + * @see + * https://members.nfc-forum.org/apps/group_public/download.php/18688/NFCForum-AD-BTSSP_1_1.pdf + * @param carrier_data_ref Reference to the carrier data record, which is the + * record that contains the actual bluetooth data. This should be the + * same as the id of the carrier data record, such as '0'. + * @return NDEF record object. + + """ + pass + + @staticmethod + def make_handover_request(carrier_data_ref: int) -> Ndef: + """* + * @brief Create a Handover request record for a Bluetooth device. + * @see + * https://members.nfc-forum.org/apps/group_public/download.php/18688/NFCForum-AD-BTSSP_1_1.pdf + * @param carrier_data_ref Reference to the carrier data record, which is the + * record that contains the actual bluetooth data. This should be the + * same as the id of the carrier data record, such as '0'. + * @return NDEF record object. + + """ + pass + + @staticmethod + def make_alternative_carrier( + power_state: Ndef.CarrierPowerState, + carrier_data_ref: int + ) -> Ndef: + """* + * @brief Create a Handover Request record for a Bluetooth device. + * @details See page 18 of https://core.ac.uk/download/pdf/250136576.pdf for more details. + * @param power_state Power state of the alternative carrier. + * @param carrier_data_ref Reference to the carrier data record, which is the + * record that contains the actual bluetooth data. This should be the + * same as the id of the carrier data record, such as '0'. + * @return NDEF record object. + + """ + pass + + @staticmethod + def make_oob_pairing( + mac_addr: int, + device_class: int, + name: std.string_view, + random_value: std.string_view = "", + confirm_value: std.string_view = "" + ) -> Ndef: + """* + * @brief Static function to make an NDEF record for BT classic OOB Pairing (Android). + * @param mac_addr 48 bit MAC Address of the BT radio + * @note If the address is e.g. f4:12:fa:42:fe:9e then the mac_addr should be + * 0xf412a42e9e. + * @param device_class The bluetooth device class for this radio. + * @param name Name of the BT device. + * @param random_value The Simple pairing randomizer R for the pairing. + * @param confirm_value The Simple pairing hash C (confirm value) for the + * pairing. + * @return NDEF record object. + + """ + pass + + @staticmethod + def make_le_oob_pairing( + mac_addr: int, + role: Ndef.BleRole, + name: std.string_view = "", + appearance: Ndef.BtAppearance = Ndef.BtAppearance.unknown, + random_value: std.string_view = "", + confirm_value: std.string_view = "", + tk: std.string_view = "" + ) -> Ndef: + """* + * @brief Static function to make an NDEF record for BLE OOB Pairing (Android). + * @param mac_addr 48 bit MAC Address of the BLE radio. + * @note If the address is e.g. f4:12:fa:42:fe:9e then the mac_addr should be + * 0xf412a42e9e. + * @param role The BLE role of the device (central / peripheral / dual) + * @param name Name of the BLE device. Optional. + * @param appearance BtAppearance of the device. Optional. + * @param random_value The Simple pairing randomizer R for the pairing. (16 bytes, optional) + * @param confirm_value The Simple pairing hash C (confirm value) for the pairing. (16 bytes, + * optional) + * @param tk Temporary key for the pairing (16 bytes, optional) + * @return NDEF record object. + + """ + pass + + def serialize(self, message_begin: bool = True, message_end: bool = True) -> List[int]: + """* + * @brief Serialize the NDEF record into a sequence of bytes. + * @param message_begin True if this is the first record in the message. + * @param message_end True if this is the last record in the message. + * @return The vector of bytes representing the NDEF record. + + """ + pass + + def payload(self) -> List[int]: + """* + * @brief Return just the payload as a vector of bytes. + * @return Payload of the NDEF record as a vector of bytes. + + """ + pass + + def set_id(self, id: int) -> None: + """* + * @brief Set the payload ID of the NDEF record. + * @param id ID of the NDEF record. + + """ + pass + + def get_id(self) -> int: + """* + * @brief Get the ID of the NDEF record. + * @return ID of the NDEF record. + + """ + pass + + def get_size(self) -> int: + """* + * @brief Get the number of bytes needed for the NDEF record. + * @return Size of the NDEF record (bytes), for serialization. + + """ + pass + + +#################### #################### + + +#################### #################### + + + +class Pid: + """* + * @brief Simple PID (proportional, integral, derivative) controller class + * with integrator clamping, output clamping, and prevention of + * integrator windup during output saturation. This class is + * thread-safe, so you can update(), clear(), and change_gains() from + * multiple threads if needed. + * + * \section pid_ex1 Basic PID Example + * \snippet pid_example.cpp pid example + * \section pid_ex2 Complex PID Example + * \snippet pid_example.cpp complex pid example + + """ + class Config: + kp: float #*< Proportional gain. + ki: float #*< Integral gain. @note should not be pre-multiplied by the time constant. + kd: float #*< Derivative gain. @note should not be pre-divided by the time-constant. + integrator_min: float #*< Minimum value the integrator can wind down to. @note Operates at the + same scale as \p output_min and \p output_max. Could be 0 or negative. + Can have different magnitude from integrator_max for asymmetric + response. + integrator_max: float #*< Maximum value the integrator can wind up to. @note Operates at the + same scale as \p output_min and \p output_max. + output_min: float #*< Limit the minimum output value. Can be a different magnitude from output + max for asymmetric output behavior. + output_max: float #*< Limit the maximum output value. + log_level: Logger.Verbosity = Logger.Verbosity(Logger.Verbosity.warn) #*< Verbosity for the adc logger. + def __init__( + self, + kp: float = float(), + ki: float = float(), + kd: float = float(), + integrator_min: float = float(), + integrator_max: float = float(), + output_min: float = float(), + output_max: float = float(), + log_level: Logger.Verbosity = Logger.Verbosity(Logger.Verbosity.warn) + ) -> None: + """Auto-generated default constructor with named params""" + pass + + + def __call__(self, error: float) -> float: + """* + * @brief Update the PID controller with the latest error measurement, + * getting the output control signal in return. + * + * @note Tracks invocation timing to better compute time-accurate + * integral/derivative signals. + * + * @param error Latest error signal. + * @return The output control signal based on the PID state and error. + + """ + pass + + def get_error(self) -> float: + """* + * @brief Get the current error (as of the last time update() or operator() + * were called) + * @return Most recent error. + + """ + pass + + def get_integrator(self) -> float: + """* + * @brief Get the current integrator (as of the last time update() or + * operator() were called) + * @return Most recent integrator value. + + """ + pass + + def get_config(self) -> Pid.Config: + """* + * @brief Get the configuration for the PID (gains, etc.). + * @return Config structure containing gains, etc. + + """ + pass + + def __init__(self) -> None: + """Auto-generated default constructor""" + pass + + +#################### #################### + + +#################### #################### + + + + + + +class Socket: + """* + * @brief Class for a generic socket with some helper functions for + * configuring the socket. + + """ + class Type(enum.IntEnum): + raw = enum.auto() # (= SOCK_RAW) #*< Only IP headers, no TCP or UDP headers as well. + dgram = enum.auto() # (= SOCK_DGRAM) #*< UDP/IP socket - datagram. + stream = enum.auto() # (= SOCK_STREAM) #*< TCP/IP socket - stream. + + class Info: + """* + * @brief Storage for socket information (address, port) with convenience + * functions to convert to/from POSIX structures. + + """ + address: str #*< IP address of the endpoint as a string. + port: int #*< Port of the endpoint as an integer. + + def init_ipv4(self, addr: str, prt: int) -> None: + """* + * @brief Initialize the struct as an ipv4 address/port combo. + * @param addr IPv4 address string + * @param prt port number + + """ + pass + + def ipv4_ptr(self) -> struct sockaddr_in: + """* + * @brief Gives access to IPv4 sockaddr structure (sockaddr_in) for use + * with low level socket calls like sendto / recvfrom. + * @return *sockaddr_in pointer to ipv4 data structure + + """ + pass + + def ipv6_ptr(self) -> struct sockaddr_in6: + """* + * @brief Gives access to IPv6 sockaddr structure (sockaddr_in6) for use + * with low level socket calls like sendto / recvfrom. + * @return *sockaddr_in6 pointer to ipv6 data structure + + """ + pass + + def update(self) -> None: + """* + * @brief Will update address and port based on the curent data in raw. + + """ + pass + + @overload + def from_sockaddr(self, source_address: struct sockaddr_storage) -> None: + """* + * @brief Fill this Info from the provided sockaddr struct. + * @param &source_address sockaddr info filled out by recvfrom. + + """ + pass + + @overload + def from_sockaddr(self, source_address: struct sockaddr_in) -> None: + """* + * @brief Fill this Info from the provided sockaddr struct. + * @param &source_address sockaddr info filled out by recvfrom. + + """ + pass + + @overload + def from_sockaddr(self, source_address: struct sockaddr_in6) -> None: + """* + * @brief Fill this Info from the provided sockaddr struct. + * @param &source_address sockaddr info filled out by recvfrom. + + """ + pass + def __init__(self, address: str = "", port: int = int()) -> None: + """Auto-generated default constructor with named params""" + pass + + + + + + + def is_valid(self) -> bool: + """* + * @brief Is the socket valid. + * @return True if the socket file descriptor is >= 0. + + """ + pass + + @staticmethod + def is_valid_fd(socket_fd: sock_type_t) -> bool: + """* + * @brief Is the socket valid. + * @param socket_fd Socket file descriptor. + * @return True if the socket file descriptor is >= 0. + + """ + pass + + def get_ipv4_info(self) -> Optional[Socket.Info]: + """* + * @brief Get the Socket::Info for the socket. + * @details This will call getsockname() on the socket to get the + * sockaddr_storage structure, and then fill out the Socket::Info + * structure. + * @return Socket::Info for the socket. + + """ + pass + + def set_receive_timeout(self, timeout: std.chrono.duration[float]) -> bool: + """* + * @brief Set the receive timeout on the provided socket. + * @param timeout requested timeout, must be > 0. + * @return True if SO_RECVTIMEO was successfully set. + + """ + pass + + def enable_reuse(self) -> bool: + """* + * @brief Allow others to use this address/port combination after we're done + * with it. + * @return True if SO_REUSEADDR and SO_REUSEPORT were successfully set. + + """ + pass + + def make_multicast(self, time_to_live: int = 1, loopback_enabled: int = True) -> bool: + """* + * @brief Configure the socket to be multicast (if time_to_live > 0). + * Sets the IP_MULTICAST_TTL (number of multicast hops allowed) and + * optionally configures whether this node should receive its own + * multicast packets (IP_MULTICAST_LOOP). + * @param time_to_live number of multicast hops allowed (TTL). + * @param loopback_enabled Whether to receive our own multicast packets. + * @return True if IP_MULTICAST_TTL and IP_MULTICAST_LOOP were set. + + """ + pass + + def add_multicast_group(self, multicast_group: str) -> bool: + """* + * @brief If this is a server socket, add it to the provided the multicast + * group. + * + * @note Multicast groups must be Class D addresses (224.0.0.0 to + * 239.255.255.255) + * + * See https://en.wikipedia.org/wiki/Multicast_address for more + * information. + * @param multicast_group multicast group to join. + * @return True if IP_ADD_MEMBERSHIP was successfully set. + + """ + pass + + def select(self, timeout: std.chrono.microseconds) -> int: + """* + * @brief Select on the socket for read events. + * @param timeout how long to wait for an event. + * @return number of events that occurred. + + """ + pass + + def __init__(self) -> None: + """Auto-generated default constructor""" + pass + + +#################### #################### + + +#################### #################### + + + + + +class TcpSocket: + """* + * @brief Class for managing sending and receiving data using TCP/IP. Can be + * used to create client or server sockets. + * + * \section tcp_ex1 TCP Client Example + * \snippet socket_example.cpp TCP Client example + * \section tcp_ex2 TCP Server Example + * \snippet socket_example.cpp TCP Server example + * + * \section tcp_ex3 TCP Client Response Example + * \snippet socket_example.cpp TCP Client Response example + * \section tcp_ex4 TCP Server Response Example + * \snippet socket_example.cpp TCP Server Response example + * + + """ + class Config: + """* + * @brief Config struct for the TCP socket. + + """ + log_level: Logger.Verbosity = Logger.Verbosity(Logger.Verbosity.warn) #*< Verbosity level for the TCP socket logger. + def __init__( + self, + log_level: Logger.Verbosity = Logger.Verbosity(Logger.Verbosity.warn) + ) -> None: + """Auto-generated default constructor with named params""" + pass + + class ConnectConfig: + """* + * @brief Config struct for connecting to a remote TCP server. + + """ + ip_address: str #*< Address to send data to. + port: int #*< Port number to send data to. + def __init__(self, ip_address: str = "", port: int = int()) -> None: + """Auto-generated default constructor with named params""" + pass + + class TransmitConfig: """* * @brief Config struct for sending data to a remote TCP socket. * @note This is only used when waiting for a response from the remote. @@ -2750,6 +3287,10 @@ class UdpSocket: + def stop_receiving(self) -> None: + """/ Stop the receive task, if one is running, and close the socket.""" + pass + @overload def send(self, data: List[int], send_config: UdpSocket.SendConfig) -> bool: """* @@ -3463,1232 +4004,1251 @@ class RtpPayloadChunk: #################### #################### -#################### #################### +#################### #################### -class RtpJpegPacket: - """/ RTP packet for JPEG video. - / The RTP payload for JPEG is defined in RFC 2435. +class RtpDepacketizer: + """/ Abstract base class for reassembling media frames from incoming RTP packets. + / Concrete depacketizers (e.g. MJPEG, H.264) override process_packet() to + / accumulate payload data and invoke the frame callback when a complete frame + / has been assembled. """ - @overload - def __init__(self, data: std.span[ int]) -> None: - """/ Construct an RTP packet from a buffer. - / @param data The buffer containing the RTP packet. - """ - pass - @overload - def __init__( - self, - type_specific: int, - frag_type: int, - q: int, - width: int, - height: int, - q0: std.span[ int], - q1: std.span[ int], - scan_data: std.span[ int] - ) -> None: - """/ Construct an RTP packet from fields - / @details This will construct a packet with quantization tables, so it - / can only be used for the first packet in a frame. - / @param type_specific The type-specific field. - / @param frag_type The fragment type field. - / @param q The q field. - / @param width The width field. - / @param height The height field. - / @param q0 The first quantization table. - / @param q1 The second quantization table. - / @param scan_data The scan data. - """ - pass + class Config: + """/ Configuration for RtpDepacketizer.""" + log_level: Logger.Verbosity = Logger.Verbosity(Logger.Verbosity.warn) #/< Log verbosity level + def __init__( + self, + log_level: Logger.Verbosity = Logger.Verbosity(Logger.Verbosity.warn) + ) -> None: + """Auto-generated default constructor with named params""" + pass - @overload - def __init__( - self, - type_specific: int, - offset: int, - frag_type: int, - q: int, - width: int, - height: int, - scan_data: std.span[ int] - ) -> None: - """/ Construct an RTP packet from fields - / @details This will construct a packet without quantization tables, so it - / cannot be used for the first packet in a frame. - / @param type_specific The type-specific field. - / @param offset The offset field. - / @param frag_type The fragment type field. - / @param q The q field. - / @param width The width field. - / @param height The height field. - / @param scan_data The scan data. - """ - pass - def get_type_specific(self) -> int: - """/ Get the type-specific field. - / @return The type-specific field. + def process_packet(self, packet: RtpPacket) -> None: + """/ Process an incoming RTP packet, accumulating payload data. + / When a complete frame is assembled the frame callback is invoked. + / @param packet The RTP packet to process. """ pass - def get_offset(self) -> int: - """/ Get the offset field. - / @return The offset field. + def set_frame_callback(self, cb: frame_callback_t) -> None: + """/ Set the callback for completed frames. + / @param cb The callback to invoke when a full frame is ready. """ pass - def get_q(self) -> int: - """/ Get the fragment type field. - / @return The fragment type field. - """ + def __init__(self) -> None: + """Auto-generated default constructor""" pass - def get_width(self) -> int: - """/ Get the fragment type field. - / @return The fragment type field. + +#################### #################### + + +#################### #################### + + + + +class RtpPacketizer: + """/ Abstract base class for splitting media frames into RTP payload chunks. + / Concrete packetizers (e.g. MJPEG, H.264) override the pure-virtual methods + / to produce codec-specific payloads. The RTSP server wraps each returned + / RtpPayloadChunk with an RTP header before sending. + """ + class Config: + """/ Configuration for RtpPacketizer.""" + max_payload_size: int = int(1400) #/< Maximum payload bytes per RTP packet + log_level: Logger.Verbosity = Logger.Verbosity(Logger.Verbosity.warn) #/< Log verbosity level + def __init__( + self, + max_payload_size: int = int(1400), + log_level: Logger.Verbosity = Logger.Verbosity(Logger.Verbosity.warn) + ) -> None: + """Auto-generated default constructor with named params""" + pass + + + + def packetize(self, frame_data: std.span[ int]) -> List[RtpPayloadChunk]: + """/ Packetize a complete media frame into RTP payload chunks. + / @param frame_data The raw frame bytes to packetize. + / @return A vector of RtpPayloadChunk ready to be wrapped in RTP packets. """ pass - def get_height(self) -> int: - """/ Get the fragment type field. - / @return The fragment type field. + def get_payload_type(self) -> int: + """/ Get the RTP payload type number for this codec. + / @return The RTP payload type (e.g. 26 for MJPEG, 96 for dynamic). """ pass - def get_mjpeg_header(self) -> std.span[ int]: - """/ Get the mjepg header. - / @return The mjepg header. + def get_clock_rate(self) -> int: + """/ Get the RTP clock rate for timestamp calculation. + / @return The clock rate in Hz (e.g. 90000 for video, 8000 for audio). """ pass - def has_q_tables(self) -> bool: - """/ Get whether the packet contains quantization tables. - / @note The quantization tables are optional. If they are present, the - / number of quantization tables is always 2. - / @note This check is based on the value of the q field. If the q field - / is 128-256, the packet contains quantization tables. - / @return Whether the packet contains quantization tables. + def get_sdp_media_attributes(self) -> str: + """/ Generate the SDP media-level attributes for this codec. + / @return A string containing SDP a= lines (without trailing CRLF). """ pass - def get_num_q_tables(self) -> int: - """/ Get the number of quantization tables. - / @note The quantization tables are optional. If they are present, the - / number of quantization tables is always 2. - / @note Only the first packet in a frame contains quantization tables. - / @return The number of quantization tables. + def get_sdp_media_line(self) -> str: + """/ Generate the SDP m= line for this codec. + / @return A string containing the SDP m= line (without trailing CRLF). """ pass - def get_q_table(self, index: int) -> std.span[ int]: - """/ Get the quantization table at the specified index. - / @param index The index of the quantization table. - / @return The quantization table at the specified index. - """ + def __init__(self) -> None: + """Auto-generated default constructor""" pass - def set_q_table(self, index: int, q_table: std.span[ int]) -> None: - """/ Set the quantization table at the specified index. - / @param index The index of the quantization table. - / @param q_table The quantization table to set. - / @note This will not change the size of the packet. If the index is out of - / bounds, the quantization table will not be set. + +#################### #################### + + +#################### #################### + + + + +class GenericDepacketizer: + """/ A generic RTP depacketizer that reassembles media frames from incoming RTP + / packets. It accumulates payload data until a packet with the marker bit set + / is received, then delivers the complete frame via the frame callback. If a + / packet arrives with a different RTP timestamp than the current accumulation + / buffer, the old buffer is discarded and a new one is started. + / + / This is suitable for audio codecs (PCM, G.711, Opus, etc.) or any payload + / format that uses simple marker-based framing. + / + / \section generic_depacketizer_ex1 Example + / \snippet rtsp_example.cpp generic_depacketizer_test + """ + class Config: + """/ Configuration for GenericDepacketizer.""" + log_level: Logger.Verbosity = Logger.Verbosity(Logger.Verbosity.warn) #/< Log verbosity level + def __init__( + self, + log_level: Logger.Verbosity = Logger.Verbosity(Logger.Verbosity.warn) + ) -> None: + """Auto-generated default constructor with named params""" + pass + + + + def process_packet(self, packet: RtpPacket) -> None: + """/ Process an incoming RTP packet. + / Payload data is accumulated until a packet with the marker bit set is + / received. At that point the assembled frame is delivered via the frame + / callback and the buffer is reset. + / @param packet The RTP packet to process. """ pass - def get_jpeg_data(self) -> std.span[ int]: - """/ Get the JPEG data. - / The jpeg data is the payload minus the mjpeg header and quantization - / tables. - / @return The JPEG data. - """ + def __init__(self) -> None: + """Auto-generated default constructor""" pass -#################### #################### +#################### #################### -#################### #################### +#################### #################### -class JpegFrame: - """/ A class that represents a complete JPEG frame. + +class GenericPacketizer: + """/ A generic RTP packetizer suitable for audio codecs (PCM, G.711, Opus, etc.) + / or any pre-formatted data that simply needs MTU-based chunking. It splits + / frame data into chunks of at most max_payload_size bytes and marks the last + / chunk with the RTP marker bit. / - / This class is used to collect the JPEG scans that are received in RTP - / packets and to serialize them into a complete JPEG frame. + / \section generic_packetizer_ex1 Example + / \snippet rtsp_example.cpp generic_packetizer_test """ - @overload - def __init__(self, packet: RtpJpegPacket) -> None: - """/ Construct a JpegFrame from a RtpJpegPacket. - / - / This constructor will parse the header of the packet and add the JPEG - / data to the frame. - / - / @param packet The packet to parse. - """ - pass + class Config: + """/ Configuration for GenericPacketizer.""" + max_payload_size: int = int(1400) #/< Maximum payload bytes per RTP packet + payload_type: int = int(96) #/< RTP payload type number + clock_rate: int = int(48000) #/< Clock rate in Hz for RTP timestamps + encoding_name: str = str("L16") #/< Encoding name for SDP rtpmap line + channels: int = int(1) #/< Number of audio channels + fmtp: str #/< Optional format parameters for SDP fmtp line + media_type: MediaType = MediaType(MediaType.audio) #/< Media type for the SDP m= line + log_level: Logger.Verbosity = Logger.Verbosity(Logger.Verbosity.warn) #/< Log verbosity level + def __init__( + self, + max_payload_size: int = int(1400), + payload_type: int = int(96), + clock_rate: int = int(48000), + encoding_name: str = str("L16"), + channels: int = int(1), + fmtp: str = "", + media_type: MediaType = MediaType(MediaType.audio), + log_level: Logger.Verbosity = Logger.Verbosity(Logger.Verbosity.warn) + ) -> None: + """Auto-generated default constructor with named params""" + pass - @overload - def __init__(self, data: List[int]) -> None: - """/ Construct a JpegFrame from a vector of jpeg data. - / @param data The vector containing the jpeg data. - / @note The vector must contain the complete JPEG data, including the JPEG - / header and EOI marker. - """ - pass - @overload - def __init__(self, data: std.span[ int]) -> None: - """/ Construct a JpegFrame from a span of jpeg data. - / @param data The span containing the jpeg data. - / @note The span must contain the complete JPEG data, including the JPEG - / header and EOI marker. - """ - pass - @overload - def __init__(self, data: int, size: int) -> None: - """/ Construct a JpegFrame from buffer of jpeg data - / @param data The buffer containing the jpeg data. - / @param size The size of the buffer. + def packetize(self, frame_data: std.span[ int]) -> List[RtpPayloadChunk]: + """/ Split frame data into RTP payload chunks of at most max_payload_size. + / The last (or only) chunk has its marker flag set. + / @param frame_data The raw frame bytes to packetize. + / @return A vector of RtpPayloadChunk ready to be wrapped in RTP packets. """ pass - def get_header(self) -> JpegHeader: - """/ Get a reference to the header. - / @return A reference to the header. + def get_payload_type(self) -> int: + """/ Get the RTP payload type number. + / @return The configured RTP payload type. """ pass - def get_width(self) -> int: - """/ Get the width of the frame. - / @return The width of the frame. + def get_clock_rate(self) -> int: + """/ Get the RTP clock rate. + / @return The configured clock rate in Hz. """ pass - def get_height(self) -> int: - """/ Get the height of the frame. - / @return The height of the frame. + def get_sdp_media_attributes(self) -> str: + """/ Generate the SDP media-level attribute lines for this codec. + / Produces an a=rtpmap line and, if fmtp is non-empty, an a=fmtp line. + / @return A string containing the SDP a= lines. """ pass - def is_complete(self) -> bool: - """/ Check if the frame is complete. - / @return True if the frame is complete, False otherwise. + def get_sdp_media_line(self) -> str: + """/ Generate the SDP m= line for this codec. + / @return A string such as "m=audio 0 RTP/AVP 96". """ pass - def append(self, packet: RtpJpegPacket) -> None: - """/ Append a RtpJpegPacket to the frame. - / This will add the JPEG data to the frame. - / @param packet The packet containing the scan to append. - """ + def __init__(self) -> None: + """Auto-generated default constructor""" pass - @overload - def add_scan(self, packet: RtpJpegPacket) -> None: - """/ Append a JPEG scan to the frame. - / This will add the JPEG data to the frame. - / @note If the packet contains the EOI marker, the frame will be - / finalized, and no further scans can be added. - / @param packet The packet containing the scan to append. - """ - pass - def get_data(self) -> std.span[ int]: - """/ Get the serialized data. - / This will return the serialized data. - / @return The serialized data. +#################### #################### + + +#################### #################### + + + + +class H264Depacketizer: + """/ @brief RTP depacketizer for H.264 video per RFC 6184. + / + / Reassembles H.264 access units from incoming RTP packets. Supports: + / - **Single NAL unit** packets (NAL type 1–23) + / - **STAP-A** aggregation packets (NAL type 24) + / - **FU-A** fragmentation packets (NAL type 28) + / + / When the RTP marker bit is set, the accumulated NAL units are delivered + / as one Annex B byte-stream (each NAL prefixed with 0x00 0x00 0x00 0x01) + / via the frame callback set with set_frame_callback(). + / + / \section h264_depacketizer_ex1 Example + / \snippet rtsp_example.cpp h264_depacketizer_test + """ + class Config: + """/ Configuration for the H264Depacketizer.""" + log_level: Logger.Verbosity = Logger.Verbosity(Logger.Verbosity.warn) #/< Log verbosity level + def __init__( + self, + log_level: Logger.Verbosity = Logger.Verbosity(Logger.Verbosity.warn) + ) -> None: + """Auto-generated default constructor with named params""" + pass + + + + def process_packet(self, packet: RtpPacket) -> None: + """/ Process an incoming RTP packet containing H.264 payload. + / + / Handles single NAL, STAP-A, and FU-A packet types. NAL units are + / buffered until the RTP marker bit indicates the end of an access unit, + / at which point the complete Annex B frame is delivered via the callback. + / + / @param packet The RTP packet to process. """ pass - def get_scan_data(self) -> std.span[ int]: - """/ Get the scan data. - / This will return the scan data. - / @return The scan data. - """ + def __init__(self) -> None: + """Auto-generated default constructor""" pass -#################### #################### +#################### #################### -#################### #################### +#################### #################### -class JpegHeader: - """/ A class to generate a JPEG header for a given image size and quantization tables. - / The header is generated once and then cached for future use. - / The header is generated according to the JPEG standard and is compatible with - / the ESP32 camera driver. + +class H264Packetizer: + """/ @brief RTP packetizer for H.264 video per RFC 6184. + / + / Accepts H.264 access units in Annex B byte-stream format (NAL units + / separated by 0x00000001 or 0x000001 start codes) and produces a sequence + / of RTP payload chunks suitable for transmission. + / + / Supports two NAL-unit packetization strategies: + / - **Single NAL unit mode** — NAL fits within max_payload_size. + / - **FU-A fragmentation** — NAL exceeds max_payload_size (packetization_mode >= 1). + / + / @note This class does not manage RTP headers (sequence numbers, timestamps, + / SSRC). The caller wraps each returned chunk into an RtpPacket. + / + / \section h264_packetizer_ex1 Example + / \snippet rtsp_example.cpp h264_packetizer_test """ - @overload - def __init__( - self, - width: int, - height: int, - q0_table: std.span[ int], - q1_table: std.span[ int] - ) -> None: - """/ Create a JPEG header for a given image size and quantization tables. - / @param width The image width in pixels. - / @param height The image height in pixels. - / @param q0_table The quantization table for the Y channel. - / @param q1_table The quantization table for the Cb and Cr channels. + class Config: + """/ Configuration for the H264Packetizer.""" + max_payload_size: int = int(1400) #/< Maximum payload bytes per RTP packet + payload_type: int = int(96) #/< Dynamic RTP payload type (typically 96–127). + profile_level_id: str #/< H.264 profile-level-id hex string, e.g. "42C01E". + packetization_mode: int = int(1) #/< 0 = single NAL only, 1 = non-interleaved (FU-A allowed). + sps: List[int] #/< Sequence Parameter Set raw bytes (without start code). + pps: List[int] #/< Picture Parameter Set raw bytes (without start code). + log_level: Logger.Verbosity = Logger.Verbosity(Logger.Verbosity.warn) #/< Log verbosity level + def __init__( + self, + max_payload_size: int = int(1400), + payload_type: int = int(96), + profile_level_id: str = "", + packetization_mode: int = int(1), + sps: List[int] = List[int](), + pps: List[int] = List[int](), + log_level: Logger.Verbosity = Logger.Verbosity(Logger.Verbosity.warn) + ) -> None: + """Auto-generated default constructor with named params""" + pass + + + + def packetize(self, frame_data: std.span[ int]) -> List[RtpPayloadChunk]: + """/ Packetize a complete H.264 access unit (Annex B format). + / + / The input may contain multiple NAL units separated by 3-byte or 4-byte + / start codes. Each NAL is individually packetized (single NAL or FU-A). + / The marker bit is set on the last chunk of the last NAL unit in the + / access unit. + / + / @param frame_data Raw Annex B byte-stream of one access unit. + / @return Vector of RTP payload chunks ready for transmission. """ pass - @overload - def __init__(self, data: std.span[ int]) -> None: - """/ Create a JPEG header from a given JPEG header data.""" + def packetize_nal( + self, + nal_data: std.span[ int], + is_last_nal: bool = True + ) -> List[RtpPayloadChunk]: + """/ Packetize a single pre-parsed NAL unit (no start code prefix). + / + / @param nal_data The raw NAL unit bytes (including NAL header byte). + / @param is_last_nal If True, the marker bit is set on the last chunk. + / @return Vector of RTP payload chunks for this NAL. + """ pass - - def get_width(self) -> int: - """/ Get the image width. - / @return The image width in pixels. + def set_sps_pps(self, sps: std.span[ int], pps: std.span[ int]) -> None: + """/ Update the SPS and PPS used for SDP generation. + / @param sps Sequence Parameter Set raw bytes. + / @param pps Picture Parameter Set raw bytes. """ pass - def get_height(self) -> int: - """/ Get the image height. - / @return The image height in pixels. + def get_payload_type(self) -> int: + """/ Get the RTP payload type. + / @return The dynamic payload type configured for H.264. """ pass - def size(self) -> int: - """/ Get the size of the JPEG header data. - / @return The size of the JPEG header data in bytes. - / @note This is the size of the serialized JPEG header, not the image size. + def get_clock_rate(self) -> int: + """/ Get the RTP clock rate for H.264 video. + / @return 90000 (fixed for H.264). """ pass - def get_data(self) -> std.span[ int]: - """/ Get the JPEG header data. - / @return The JPEG header data. + def get_sdp_media_attributes(self) -> str: + """/ Get the SDP attribute lines for H.264. + / @return SDP a= lines (rtpmap and fmtp) without trailing CRLF. """ pass - def get_quantization_table(self, index: int) -> std.span[ int]: - """/ Get the Quantization table at the index. - / @param index The index of the quantization table. - / @return The quantization table. + def get_sdp_media_line(self) -> str: + """/ Get the SDP m= media line for H.264. + / @return SDP m= line without trailing CRLF. """ pass - -#################### #################### - - -#################### #################### - - -class RtcpPacket: - """/ @brief A class to represent a RTCP packet - / @details This class is used to represent a RTCP packet. - / It is used as a base class for all RTCP packet types. - / @note At the moment, this class is not used. - """ def __init__(self) -> None: - """/ @brief Constructor, default""" + """Auto-generated default constructor""" pass - def get_data(self) -> std.string_view: - """/ @brief Get the buffer of the packet - / @return The buffer of the packet - """ - pass +#################### #################### -#################### #################### +#################### #################### -#################### #################### -class RtpPacket: - """/ RtpPacket is a class to parse RTP packet. - / It can be used to parse and serialize RTP packets. - / The RTP header fields are stored in the class and can be modified. - / The payload is stored in the packet_ vector and can be modified. +class MjpegDepacketizer: + """/ MJPEG depacketizer that reassembles JPEG frames from RTP packets. + / + / This class receives individual RTP packets containing RFC 2435 MJPEG + / payloads, reassembles the scan data fragments, reconstructs the JPEG + / header from the MJPEG header fields, and delivers complete JPEG frames + / through callbacks. """ - @overload - def __init__(self) -> None: - """/ Construct an empty RtpPacket. - / The packet_ vector is empty and the header fields are set to 0. - """ - pass - @overload - def __init__(self, payload_size: int) -> None: - """/ Construct an RtpPacket with a payload of size payload_size. - / The packet_ vector is resized to RTP_HEADER_SIZE + payload_size. + class Config: + """/ Configuration for the MJPEG depacketizer.""" + log_level: Logger.Verbosity = Logger.Verbosity(Logger.Verbosity.warn) #/< Log verbosity level + def __init__( + self, + log_level: Logger.Verbosity = Logger.Verbosity(Logger.Verbosity.warn) + ) -> None: + """Auto-generated default constructor with named params""" + pass + + + def set_jpeg_frame_callback(self, cb: jpeg_frame_callback_t) -> None: + """/ Set callback for receiving complete JPEG frames. + / @param cb Callback receiving a shared pointer to the completed JpegFrame. """ pass - @overload - def __init__(self, data: std.span[ int]) -> None: - """/ Construct an RtpPacket from a span of bytes. - / Stores the bytes in the packet_ vector and parses the header. - / @param data The span of bytes to parse. - """ + def __init__(self) -> None: + """Auto-generated default constructor""" pass - # ----------------------------------------------------------------- - # Getters for the RTP header fields. - # ----------------------------------------------------------------- +#################### #################### - def get_version(self) -> int: - """/ Get the RTP version. - / @return The RTP version. - """ - pass - def get_padding(self) -> bool: - """/ Get the padding flag. - / @return The padding flag. - """ - pass +#################### #################### - def get_extension(self) -> bool: - """/ Get the extension flag. - / @return The extension flag. - """ - pass - def get_csrc_count(self) -> int: - """/ Get the CSRC count. - / @return The CSRC count. - """ - pass - def get_marker(self) -> bool: - """/ Get the marker flag. - / @return The marker flag. - """ - pass +class MjpegPacketizer: + """/ MJPEG packetizer that fragments JPEG frames into RFC 2435 RTP payloads. + / + / This class takes complete JPEG frames and produces RTP payload chunks + / suitable for MJPEG streaming. Each chunk contains an RFC 2435 MJPEG + / header, and the first chunk additionally includes quantization tables. + """ + class Config: + """/ Configuration for the MJPEG packetizer.""" + max_payload_size: int = int(1400) #/< Maximum payload bytes per RTP packet + log_level: Logger.Verbosity = Logger.Verbosity(Logger.Verbosity.warn) #/< Log verbosity level + def __init__( + self, + max_payload_size: int = int(1400), + log_level: Logger.Verbosity = Logger.Verbosity(Logger.Verbosity.warn) + ) -> None: + """Auto-generated default constructor with named params""" + pass + def get_payload_type(self) -> int: - """/ Get the payload type. - / @return The payload type. + """/ Get the RTP payload type for MJPEG. + / @return 26 (static JPEG payload type). """ pass - def get_sequence_number(self) -> int: - """/ Get the sequence number. - / @return The sequence number. + def get_clock_rate(self) -> int: + """/ Get the RTP clock rate for MJPEG. + / @return 90000 Hz. """ pass - def get_timestamp(self) -> int: - """/ Get the timestamp. - / @return The timestamp. + def get_sdp_media_attributes(self) -> str: + """/ Get the SDP media attributes for MJPEG. + / @return SDP rtpmap attribute string. """ pass - def get_ssrc(self) -> int: - """/ Get the SSRC. - / @return The SSRC. + def get_sdp_media_line(self) -> str: + """/ Get the SDP media line for MJPEG. + / @return SDP media description line. """ pass + def __init__(self) -> None: + """Auto-generated default constructor""" + pass - # ----------------------------------------------------------------- - # Setters for the RTP header fields. - # ----------------------------------------------------------------- - def set_version(self, version: int) -> None: - """/ Set the RTP version. - / @param version The RTP version to set. - """ - pass +#################### #################### - def set_padding(self, padding: bool) -> None: - """/ Set the padding flag. - / @param padding The padding flag to set. - """ - pass - def set_extension(self, extension: bool) -> None: - """/ Set the extension flag. - / @param extension The extension flag to set. - """ - pass +#################### #################### - def set_csrc_count(self, csrc_count: int) -> None: - """/ Set the CSRC count. - / @param csrc_count The CSRC count to set. + + + +class RtpJpegPacket: + """/ RTP packet for JPEG video. + / The RTP payload for JPEG is defined in RFC 2435. + """ + @overload + def __init__(self, data: std.span[ int]) -> None: + """/ Construct an RTP packet from a buffer. + / @param data The buffer containing the RTP packet. """ pass - def set_marker(self, marker: bool) -> None: - """/ Set the marker flag. - / @param marker The marker flag to set. + @overload + def __init__( + self, + type_specific: int, + frag_type: int, + q: int, + width: int, + height: int, + q0: std.span[ int], + q1: std.span[ int], + scan_data: std.span[ int] + ) -> None: + """/ Construct an RTP packet from fields + / @details This will construct a packet with quantization tables, so it + / can only be used for the first packet in a frame. + / @param type_specific The type-specific field. + / @param frag_type The fragment type field. + / @param q The q field. + / @param width The width field. + / @param height The height field. + / @param q0 The first quantization table. + / @param q1 The second quantization table. + / @param scan_data The scan data. """ pass - def set_payload_type(self, payload_type: int) -> None: - """/ Set the payload type. - / @param payload_type The payload type to set. + @overload + def __init__( + self, + type_specific: int, + offset: int, + frag_type: int, + q: int, + width: int, + height: int, + scan_data: std.span[ int] + ) -> None: + """/ Construct an RTP packet from fields + / @details This will construct a packet without quantization tables, so it + / cannot be used for the first packet in a frame. + / @param type_specific The type-specific field. + / @param offset The offset field. + / @param frag_type The fragment type field. + / @param q The q field. + / @param width The width field. + / @param height The height field. + / @param scan_data The scan data. """ pass - def set_sequence_number(self, sequence_number: int) -> None: - """/ Set the sequence number. - / @param sequence_number The sequence number to set. + + def get_type_specific(self) -> int: + """/ Get the type-specific field. + / @return The type-specific field. """ pass - def set_timestamp(self, timestamp: int) -> None: - """/ Set the timestamp. - / @param timestamp The timestamp to set. + def get_offset(self) -> int: + """/ Get the offset field. + / @return The offset field. """ pass - def set_ssrc(self, ssrc: int) -> None: - """/ Set the SSRC. - / @param ssrc The SSRC to set. + def get_q(self) -> int: + """/ Get the fragment type field. + / @return The fragment type field. """ pass - # ----------------------------------------------------------------- - # Utility methods. - # ----------------------------------------------------------------- - - def serialize(self) -> None: - """/ Serialize the RTP header. - / @note This method should be called after modifying the RTP header fields. - / @note This method does not serialize the payload. To set the payload, use - / set_payload(). - / To get the payload, use get_payload(). + def get_width(self) -> int: + """/ Get the fragment type field. + / @return The fragment type field. """ pass - def get_data(self) -> std.span[ int]: - """/ Get a span view of the whole packet. - / @note The span is valid as long as the packet_ vector is not modified. - / @note If you manually build the packet_ vector, you should make sure that you - / call serialize() before calling this method. - / @return A span of the whole packet. + def get_height(self) -> int: + """/ Get the fragment type field. + / @return The fragment type field. """ pass - def get_rtp_header_size(self) -> int: - """/ Get the size of the RTP header. - / @return The size of the RTP header. + def get_mjpeg_header(self) -> std.span[ int]: + """/ Get the mjepg header. + / @return The mjepg header. """ pass - def get_rtp_header(self) -> std.span[ int]: - """/ Get a span of bytes of the RTP header. - / @return A span of bytes of the RTP header. + def has_q_tables(self) -> bool: + """/ Get whether the packet contains quantization tables. + / @note The quantization tables are optional. If they are present, the + / number of quantization tables is always 2. + / @note This check is based on the value of the q field. If the q field + / is 128-256, the packet contains quantization tables. + / @return Whether the packet contains quantization tables. """ pass - def get_packet(self) -> List[int]: - """/ Get a reference to the packet_ vector. - / @return A reference to the packet_ vector. + def get_num_q_tables(self) -> int: + """/ Get the number of quantization tables. + / @note The quantization tables are optional. If they are present, the + / number of quantization tables is always 2. + / @note Only the first packet in a frame contains quantization tables. + / @return The number of quantization tables. """ pass - def get_payload(self) -> std.span[ int]: - """/ Get a span of bytes of the payload. - / @return A span of bytes of the payload. + def get_q_table(self, index: int) -> std.span[ int]: + """/ Get the quantization table at the specified index. + / @param index The index of the quantization table. + / @return The quantization table at the specified index. """ pass - def set_payload(self, payload: std.span[ int]) -> None: - """/ Set the payload. - / @param payload The payload to set. + def set_q_table(self, index: int, q_table: std.span[ int]) -> None: + """/ Set the quantization table at the specified index. + / @param index The index of the quantization table. + / @param q_table The quantization table to set. + / @note This will not change the size of the packet. If the index is out of + / bounds, the quantization table will not be set. """ pass + def get_jpeg_data(self) -> std.span[ int]: + """/ Get the JPEG data. + / The jpeg data is the payload minus the mjpeg header and quantization + / tables. + / @return The JPEG data. + """ + pass -#################### #################### - - -#################### #################### +#################### #################### +#################### #################### -class RtspClient: - """/ A class for interacting with an RTSP server using RTP and RTCP over UDP - / - / This class is used to connect to an RTSP server and receive JPEG frames - / over RTP. It uses the TCP socket to send RTSP requests and receive RTSP - / responses. It uses the UDP socket to receive RTP and RTCP packets. - / - / The RTSP client is designed to be used with the RTSP server in the - / [camera-streamer]https://github.com/esp-cpp/camera-streamer) project, but it - / should work with any RTSP server that sends JPEG frames over RTP. +class JpegFrame: + """/ A class that represents a complete JPEG frame. / - / \section rtsp_client_ex1 RtspClient Example - / \snippet rtsp_example.cpp rtsp_client_example + / This class is used to collect the JPEG scans that are received in RTP + / packets and to serialize them into a complete JPEG frame. """ - - - class Config: - """/ Configuration for the RTSP client""" - server_address: str #/< The server IP Address to connect to - rtsp_port: int = int(8554) #/< The port of the RTSP server - path: str = str("/mjpeg/1") #/< The path to the RTSP stream on the server. Will be appended - #/< to the server address and port to form the full path of the - #/< form "rtsp://:" - - #/ Generic frame callback for any codec (track_id, raw frame data) - on_frame: frame_callback_t = frame_callback_t(None) - - #/ JPEG-specific frame callback (backward compatible). - #/ If set and no depacketizer is registered for PT 26, an MjpegDepacketizer - #/ is automatically created. - on_jpeg_frame: jpeg_frame_callback_t = jpeg_frame_callback_t(None) - - log_level: Logger.Verbosity = Logger.Verbosity.info #/< The verbosity of the logger - def __init__( - self, - server_address: str = "", - rtsp_port: int = int(8554), - path: str = str("/mjpeg/1"), - on_frame: frame_callback_t = frame_callback_t(None), - on_jpeg_frame: jpeg_frame_callback_t = jpeg_frame_callback_t(None), - log_level: Logger.Verbosity = Logger.Verbosity.info - ) -> None: - """Auto-generated default constructor with named params""" - pass - - - - def send_request( - self, - method: str, - path: str, - extra_headers: std.unordered_map[str, str], - ec: std.error_code - ) -> str: - """/ Send an RTSP request to the server - / \note This is a blocking call - / \note This will parse the response and set the session ID if it is - / present in the response. If the response is not a 200 OK, then - / an error code will be set and the response will be returned. - / If the response is a 200 OK, then the response will be returned - / and the error code will be set to success. - / \param method The method to use for connecting. - / Options are "OPTIONS", "DESCRIBE", "SETUP", "PLAY", and "TEARDOWN" - / \param path The path to the RTSP stream on the server. - / \param extra_headers Any extra headers to send with the request. These - / will be added to the request after the CSeq and Session headers. The - / key is the header name and the value is the header value. For example, - / {"Accept": "application/sdp"} will add "Accept: application/sdp" to the - / request. The "User-Agent" header will be added automatically. The - / "CSeq" and "Session" headers will be added automatically. - / The "Accept" header will be added automatically. The "Transport" - / header will be added automatically for the "SETUP" method. Defaults to - / an empty map. - / \param ec The error code to set if an error occurs - / \return The response from the server - """ - pass - - def connect(self, ec: std.error_code) -> None: - """/ Connect to the RTSP server - / Connects to the RTSP server and sends the OPTIONS request. - / \param ec The error code to set if an error occurs - """ - pass - - def disconnect(self, ec: std.error_code) -> None: - """/ Disconnect from the RTSP server - / Disconnects from the RTSP server and sends the TEARDOWN request. - / \param ec The error code to set if an error occurs - """ - pass - - def describe(self, ec: std.error_code) -> None: - """/ Describe the RTSP stream - / Sends the DESCRIBE request to the RTSP server and parses the response. - / \param ec The error code to set if an error occurs + @overload + def __init__(self, packet: RtpJpegPacket) -> None: + """/ Construct a JpegFrame from a RtpJpegPacket. + / + / This constructor will parse the header of the packet and add the JPEG + / data to the frame. + / + / @param packet The packet to parse. """ pass @overload - def setup(self, ec: std.error_code) -> None: - """/ Setup the RTSP stream - / \note Starts the RTP and RTCP threads. - / Sends the SETUP request to the RTSP server and parses the response. - / \note The default ports are 5000 and 5001 for RTP and RTCP respectively. - / \note The default receive timeout is 5 seconds. - / \param ec The error code to set if an error occurs + def __init__(self, data: List[int]) -> None: + """/ Construct a JpegFrame from a vector of jpeg data. + / @param data The vector containing the jpeg data. + / @note The vector must contain the complete JPEG data, including the JPEG + / header and EOI marker. """ pass @overload - def setup( - self, - rtp_port: int, - rtcp_port: int, - receive_timeout: std.chrono.duration[float], - ec: std.error_code - ) -> None: - """/ Setup the RTSP stream - / Sends the SETUP request to the RTSP server and parses the response. - / \note Starts the RTP and RTCP threads. - / \param rtp_port The RTP client port - / \param rtcp_port The RTCP client port - / \param receive_timeout The timeout for receiving RTP and RTCP packets - / \param ec The error code to set if an error occurs + def __init__(self, data: std.span[ int]) -> None: + """/ Construct a JpegFrame from a span of jpeg data. + / @param data The span containing the jpeg data. + / @note The span must contain the complete JPEG data, including the JPEG + / header and EOI marker. """ pass - def add_depacketizer(self, payload_type: int, depacketizer: RtpDepacketizer) -> None: - """/ Register a depacketizer for a specific RTP payload type. - / When RTP packets with this payload type are received, they are - / dispatched to the registered depacketizer. - / @param payload_type The RTP payload type (e.g., 26 for MJPEG, 96 for H264) - / @param depacketizer The depacketizer to handle packets of this type + @overload + def __init__(self, data: int, size: int) -> None: + """/ Construct a JpegFrame from buffer of jpeg data + / @param data The buffer containing the jpeg data. + / @param size The size of the buffer. """ pass - def play(self, ec: std.error_code) -> None: - """/ Play the RTSP stream - / Sends the PLAY request to the RTSP server and parses the response. - / \param ec The error code to set if an error occurs + def get_header(self) -> JpegHeader: + """/ Get a reference to the header. + / @return A reference to the header. """ pass - def pause(self, ec: std.error_code) -> None: - """/ Pause the RTSP stream - / Sends the PAUSE request to the RTSP server and parses the response. - / \param ec The error code to set if an error occurs + def get_width(self) -> int: + """/ Get the width of the frame. + / @return The width of the frame. """ pass - def teardown(self, ec: std.error_code) -> None: - """/ Teardown the RTSP stream - / Sends the TEARDOWN request to the RTSP server and parses the response. - / \param ec The error code to set if an error occurs + def get_height(self) -> int: + """/ Get the height of the frame. + / @return The height of the frame. """ pass - def __init__(self) -> None: - """Auto-generated default constructor""" + def is_complete(self) -> bool: + """/ Check if the frame is complete. + / @return True if the frame is complete, False otherwise. + """ pass + def append(self, packet: RtpJpegPacket) -> None: + """/ Append a RtpJpegPacket to the frame. + / This will add the JPEG data to the frame. + / @param packet The packet containing the scan to append. + """ + pass -#################### #################### + @overload + def add_scan(self, packet: RtpJpegPacket) -> None: + """/ Append a JPEG scan to the frame. + / This will add the JPEG data to the frame. + / @note If the packet contains the EOI marker, the frame will be + / finalized, and no further scans can be added. + / @param packet The packet containing the scan to append. + """ + pass + def get_data(self) -> std.span[ int]: + """/ Get the serialized data. + / This will return the serialized data. + / @return The serialized data. + """ + pass -#################### #################### + def get_scan_data(self) -> std.span[ int]: + """/ Get the scan data. + / This will return the scan data. + / @return The scan data. + """ + pass +#################### #################### +#################### #################### -class RtspServer: - """/ Class for streaming MJPEG data from a camera using RTSP + RTP - / Starts a TCP socket to listen for RTSP connections, and then spawns off a - / new RTSP session for each connection. - / @see RtspSession - / @note This class does not currently send RTCP packets - / - / \section rtsp_server_ex1 RtspServer example - / \snippet rtsp_example.cpp rtsp_server_example +class JpegHeader: + """/ A class to generate a JPEG header for a given image size and quantization tables. + / The header is generated once and then cached for future use. + / The header is generated according to the JPEG standard and is compatible with + / the ESP32 camera driver. """ - class Config: - """/ @brief Configuration for the RTSP server""" - server_address: str #/< The ip address of the server - port: int #/< The port to listen on - path: str #/< The path to the RTSP stream - max_data_size: int = 1000 #/< The maximum size of RTP packet data for the MJPEG stream. Frames will be broken - #/< up into multiple packets if they are larger than this. It seems that 1500 works - #/< well for sending, but is too large for the esp32 (camera-display) to receive - #/< properly. - log_level: Logger.Verbosity = Logger.Verbosity.warn #/< The log level for the RTSP server - def __init__( - self, - server_address: str = "", - port: int = int(), - path: str = "", - max_data_size: int = 1000, - log_level: Logger.Verbosity = Logger.Verbosity.warn - ) -> None: - """Auto-generated default constructor with named params""" - pass - - class TrackConfig: - """/ Configuration for a media track to be registered with the server""" - track_id: int = int(0) #/< Track identifier - packetizer: RtpPacketizer #/< Codec-specific packetizer - def __init__( - self, - track_id: int = int(0), - packetizer: RtpPacketizer = RtpPacketizer() - ) -> None: - """Auto-generated default constructor with named params""" - pass + @overload + def __init__( + self, + width: int, + height: int, + q0_table: std.span[ int], + q1_table: std.span[ int] + ) -> None: + """/ Create a JPEG header for a given image size and quantization tables. + / @param width The image width in pixels. + / @param height The image height in pixels. + / @param q0_table The quantization table for the Y channel. + / @param q1_table The quantization table for the Cb and Cr channels. + """ + pass + @overload + def __init__(self, data: std.span[ int]) -> None: + """/ Create a JPEG header from a given JPEG header data.""" + pass - def set_session_log_level(self, log_level: Logger.Verbosity) -> None: - """/ @brief Sets the log level for the RTSP sessions created by this server - / @note This does not affect the log level of the RTSP server itself - / @note This does not change the log level of any sessions that have - / already been created - / @param log_level The log level to set + def get_width(self) -> int: + """/ Get the image width. + / @return The image width in pixels. """ pass - def start( - self, - accept_timeout: std.chrono.duration[float] = std.chrono.seconds(5) - ) -> bool: - """/ @brief Start the RTSP server - / Starts the accept task, session task, and binds the RTSP socket - / @param accept_timeout The timeout for accepting new connections - / @return True if the server was started successfully, False otherwise + def get_height(self) -> int: + """/ Get the image height. + / @return The image height in pixels. """ pass - def stop(self) -> None: - """/ @brief Stop the FTP server - / Stops the accept task, session task, and closes the RTSP socket + def size(self) -> int: + """/ Get the size of the JPEG header data. + / @return The size of the JPEG header data in bytes. + / @note This is the size of the serialized JPEG header, not the image size. """ pass - def add_track(self, config: RtspServer.TrackConfig) -> None: - """/ @brief Register a media track with the server. - / Each track has its own packetizer, SSRC, and sequence number. - / @param config Track configuration including the packetizer. - """ + def is_valid(self) -> bool: + """/ Returns whether this header parsed or serialized successfully.""" pass - @overload - def send_frame(self, track_id: int, frame_data: std.span[ int]) -> None: - """/ @brief Send a frame on a specific track. - / The track's packetizer splits the frame into RTP payload chunks, - / which are then wrapped with RTP headers and queued for delivery. - / @note Overwrites any existing pending packets for this track. - / @param track_id The track to send on. - / @param frame_data Raw encoded frame data. + def get_data(self) -> std.span[ int]: + """/ Get the JPEG header data. + / @return The JPEG header data. """ pass - @overload - def send_frame(self, frame: JpegFrame) -> None: - """/ @brief Send a JPEG frame over the RTSP connection (backward compatible). - / If no tracks have been added, lazily creates a default MJPEG track on - / track 0. Uses the legacy RtpJpegPacket packetization to preserve the - / exact wire format for existing MJPEG users. - / @note Overwrites any existing frame that has not been sent. - / @param frame The frame to send. + def get_quantization_table(self, index: int) -> std.span[ int]: + """/ Get the Quantization table at the index. + / @param index The index of the quantization table. + / @return The quantization table. """ pass + +#################### #################### + + +#################### #################### + + +class RtcpPacket: + """/ @brief A class to represent a RTCP packet + / @details This class is used to represent a RTCP packet. + / It is used as a base class for all RTCP packet types. + / @note At the moment, this class is not used. + """ def __init__(self) -> None: - """Auto-generated default constructor""" + """/ @brief Constructor, default""" pass -#################### #################### - -#################### #################### + def get_data(self) -> std.string_view: + """/ @brief Get the buffer of the packet + / @return The buffer of the packet + """ + pass +#################### #################### +#################### #################### -class RtspSession: - """/ Class that reepresents an RTSP session, which is uniquely identified by a - / session id and sends frame data over RTP and RTCP to the client +class RtpPacket: + """/ RtpPacket is a class to parse RTP packet. + / It can be used to parse and serialize RTP packets. + / The RTP header fields are stored in the class and can be modified. + / The payload is stored in the packet_ vector and can be modified. """ - class Track: - """/ Represents one media track within an RTSP session""" - track_id: int = int(0) #/< Track identifier (matches trackID=N in SDP) - control_path: str #/< Control path suffix (e.g., "trackID=0") - rtp_socket: UdpSocket #/< RTP socket for this track - rtcp_socket: UdpSocket #/< RTCP socket for this track - client_rtp_port: int = int(0) #/< Client's RTP port - client_rtcp_port: int = int(0) #/< Client's RTCP port - setup_complete: bool = bool(False) #/< Whether SETUP has been completed for this track + @overload + def __init__(self) -> None: + """/ Construct an empty RtpPacket. + / The packet_ vector is empty and the header fields are set to 0. + """ + pass - def __init__(self) -> None: - pass + @overload + def __init__(self, payload_size: int) -> None: + """/ Construct an RtpPacket with a payload of size payload_size. + / The packet_ vector is resized to RTP_HEADER_SIZE + payload_size. + """ + pass - class Config: - """/ Configuration for the RTSP session""" - server_address: str #/< The address of the server - rtsp_path: str #/< The RTSP path of the session - receive_timeout: std.chrono.duration[float] = std.chrono.seconds(5) #/< The timeout for receiving data. Should be > 0. - #/ SDP generator callback. If set, called during DESCRIBE to produce the SDP body. - #/ If not set, a default MJPEG SDP is generated for backward compatibility. - #/ @param session_path Full RTSP path (e.g., "rtsp://ip:port/path") - #/ @param session_id The session ID - #/ @param server_address The server address with port - sdp_generator: std.function[str( str session_path, int session_id, str server_address)] - log_level: Logger.Verbosity = Logger.Verbosity.warn #/< The log level of the session - def __init__( - self, - server_address: str = "", - rtsp_path: str = "", - receive_timeout: std.chrono.duration[float] = std.chrono.seconds(5), - log_level: Logger.Verbosity = Logger.Verbosity.warn - ) -> None: - """Auto-generated default constructor with named params""" - pass + @overload + def __init__(self, data: std.span[ int]) -> None: + """/ Construct an RtpPacket from a span of bytes. + / Stores the bytes in the packet_ vector and parses the header. + / @param data The span of bytes to parse. + """ + pass + # ----------------------------------------------------------------- + # Getters for the RTP header fields. + # ----------------------------------------------------------------- - def get_session_id(self) -> int: - """/ @brief Get the session id - / @return The session id + def get_version(self) -> int: + """/ Get the RTP version. + / @return The RTP version. """ pass - def is_closed(self) -> bool: - """/ @brief Check if the session is closed - / @return True if the session is closed, False otherwise + def get_padding(self) -> bool: + """/ Get the padding flag. + / @return The padding flag. """ pass - def is_connected(self) -> bool: - """/ Get whether the session is connected - / @return True if the session is connected, False otherwise + def get_extension(self) -> bool: + """/ Get the extension flag. + / @return The extension flag. """ pass - def is_active(self) -> bool: - """/ Get whether the session is active - / @return True if the session is active, False otherwise + def get_csrc_count(self) -> int: + """/ Get the CSRC count. + / @return The CSRC count. """ pass - def play(self) -> None: - """/ Mark the session as active - / This will cause the server to start sending frames to the client + def get_marker(self) -> bool: + """/ Get the marker flag. + / @return The marker flag. """ pass - def pause(self) -> None: - """/ Pause the session - / This will cause the server to stop sending frames to the client - / @note This does not stop the session, it just pauses it - / @note This is useful for when the client is buffering + def get_payload_type(self) -> int: + """/ Get the payload type. + / @return The payload type. """ pass - def teardown(self) -> None: - """/ Teardown the session - / This will cause the server to stop sending frames to the client - / and close the connection + def get_sequence_number(self) -> int: + """/ Get the sequence number. + / @return The sequence number. """ pass - @overload - def send_rtp_packet(self, track_id: int, packet: RtpPacket) -> bool: - """/ Send an RTP packet on a specific track - / @param track_id The track to send on - / @param packet The RTP packet to send - / @return True if the packet was sent successfully, False otherwise + def get_timestamp(self) -> int: + """/ Get the timestamp. + / @return The timestamp. """ pass - @overload - def send_rtp_packet(self, packet: RtpPacket) -> bool: - """/ Send an RTP packet to the client (backward compat — sends on default track 0) - / @param packet The RTP packet to send - / @return True if the packet was sent successfully, False otherwise + def get_ssrc(self) -> int: + """/ Get the SSRC. + / @return The SSRC. """ pass - @overload - def send_rtcp_packet(self, track_id: int, packet: RtcpPacket) -> bool: - """/ Send an RTCP packet on a specific track - / @param track_id The track to send on - / @param packet The RTCP packet to send - / @return True if the packet was sent successfully, False otherwise + # ----------------------------------------------------------------- + # Setters for the RTP header fields. + # ----------------------------------------------------------------- + + def set_version(self, version: int) -> None: + """/ Set the RTP version. + / @param version The RTP version to set. """ pass - @overload - def send_rtcp_packet(self, packet: RtcpPacket) -> bool: - """/ Send an RTCP packet to the client (backward compat — sends on default track 0) - / @param packet The RTCP packet to send - / @return True if the packet was sent successfully, False otherwise + def set_padding(self, padding: bool) -> None: + """/ Set the padding flag. + / @param padding The padding flag to set. """ pass - def __init__(self) -> None: - """Auto-generated default constructor""" + def set_extension(self, extension: bool) -> None: + """/ Set the extension flag. + / @param extension The extension flag to set. + """ pass -#################### #################### - - -#################### #################### - - - - -class GenericDepacketizer: - """/ A generic RTP depacketizer that reassembles media frames from incoming RTP - / packets. It accumulates payload data until a packet with the marker bit set - / is received, then delivers the complete frame via the frame callback. If a - / packet arrives with a different RTP timestamp than the current accumulation - / buffer, the old buffer is discarded and a new one is started. - / - / This is suitable for audio codecs (PCM, G.711, Opus, etc.) or any payload - / format that uses simple marker-based framing. - / - / \section generic_depacketizer_ex1 Example - / \snippet generic_depacketizer_example.cpp generic_depacketizer example - """ - class Config: - """/ Configuration for GenericDepacketizer.""" - log_level: Logger.Verbosity = Logger.Verbosity(Logger.Verbosity.warn) #/< Log verbosity level - def __init__( - self, - log_level: Logger.Verbosity = Logger.Verbosity(Logger.Verbosity.warn) - ) -> None: - """Auto-generated default constructor with named params""" - pass - - - - def process_packet(self, packet: RtpPacket) -> None: - """/ Process an incoming RTP packet. - / Payload data is accumulated until a packet with the marker bit set is - / received. At that point the assembled frame is delivered via the frame - / callback and the buffer is reset. - / @param packet The RTP packet to process. + def set_csrc_count(self, csrc_count: int) -> None: + """/ Set the CSRC count. + / @param csrc_count The CSRC count to set. """ pass - def __init__(self) -> None: - """Auto-generated default constructor""" + def set_marker(self, marker: bool) -> None: + """/ Set the marker flag. + / @param marker The marker flag to set. + """ pass + def set_payload_type(self, payload_type: int) -> None: + """/ Set the payload type. + / @param payload_type The payload type to set. + """ + pass -#################### #################### + def set_sequence_number(self, sequence_number: int) -> None: + """/ Set the sequence number. + / @param sequence_number The sequence number to set. + """ + pass + def set_timestamp(self, timestamp: int) -> None: + """/ Set the timestamp. + / @param timestamp The timestamp to set. + """ + pass -#################### #################### + def set_ssrc(self, ssrc: int) -> None: + """/ Set the SSRC. + / @param ssrc The SSRC to set. + """ + pass + # ----------------------------------------------------------------- + # Utility methods. + # ----------------------------------------------------------------- + def serialize(self) -> None: + """/ Serialize the RTP header. + / @note This method should be called after modifying the RTP header fields. + / @note This method does not serialize the payload. To set the payload, use + / set_payload(). + / To get the payload, use get_payload(). + """ + pass + def get_data(self) -> std.span[ int]: + """/ Get a span view of the whole packet. + / @note The span is valid as long as the packet_ vector is not modified. + / @note If you manually build the packet_ vector, you should make sure that you + / call serialize() before calling this method. + / @return A span of the whole packet. + """ + pass -class H264Depacketizer: - """/ @brief RTP depacketizer for H.264 video per RFC 6184. - / - / Reassembles H.264 access units from incoming RTP packets. Supports: - / - **Single NAL unit** packets (NAL type 1–23) - / - **STAP-A** aggregation packets (NAL type 24) - / - **FU-A** fragmentation packets (NAL type 28) - / - / When the RTP marker bit is set, the accumulated NAL units are delivered - / as one Annex B byte-stream (each NAL prefixed with 0x00 0x00 0x00 0x01) - / via the frame callback set with set_frame_callback(). - / - / \section h264_depacketizer_ex1 Example - / \snippet h264_depacketizer_example.cpp h264_depacketizer example - """ - class Config: - """/ Configuration for the H264Depacketizer.""" - log_level: Logger.Verbosity = Logger.Verbosity(Logger.Verbosity.warn) #/< Log verbosity level - def __init__( - self, - log_level: Logger.Verbosity = Logger.Verbosity(Logger.Verbosity.warn) - ) -> None: - """Auto-generated default constructor with named params""" - pass + def get_rtp_header_size(self) -> int: + """/ Get the size of the RTP header. + / @return The size of the RTP header. + """ + pass + def get_rtp_header(self) -> std.span[ int]: + """/ Get a span of bytes of the RTP header. + / @return A span of bytes of the RTP header. + """ + pass + def get_packet(self) -> List[int]: + """/ Get a reference to the packet_ vector. + / @return A reference to the packet_ vector. + """ + pass - def process_packet(self, packet: RtpPacket) -> None: - """/ Process an incoming RTP packet containing H.264 payload. - / - / Handles single NAL, STAP-A, and FU-A packet types. NAL units are - / buffered until the RTP marker bit indicates the end of an access unit, - / at which point the complete Annex B frame is delivered via the callback. - / - / @param packet The RTP packet to process. + def get_payload(self) -> std.span[ int]: + """/ Get a span of bytes of the payload. + / @return A span of bytes of the payload. """ pass - def __init__(self) -> None: - """Auto-generated default constructor""" + def set_payload(self, payload: std.span[ int]) -> None: + """/ Set the payload. + / @param payload The payload to set. + """ pass -#################### #################### +#################### #################### -#################### #################### +#################### #################### -class MjpegDepacketizer: - """/ MJPEG depacketizer that reassembles JPEG frames from RTP packets. + + +class RtspClient: + """/ A class for interacting with an RTSP server using RTP and RTCP over UDP / - / This class receives individual RTP packets containing RFC 2435 MJPEG - / payloads, reassembles the scan data fragments, reconstructs the JPEG - / header from the MJPEG header fields, and delivers complete JPEG frames - / through callbacks. + / This class is used to connect to an RTSP server and receive JPEG frames + / over RTP. It uses the TCP socket to send RTSP requests and receive RTSP + / responses. It uses the UDP socket to receive RTP and RTCP packets. + / + / The RTSP client is designed to be used with the RTSP server in the + / [camera-streamer]https://github.com/esp-cpp/camera-streamer) project, but it + / should work with any RTSP server that sends JPEG frames over RTP. + / + / \section rtsp_client_ex1 RtspClient Example + / \snippet rtsp_example.cpp rtsp_client_example """ - - class Config: - """/ Configuration for the MJPEG depacketizer.""" - log_level: Logger.Verbosity = Logger.Verbosity(Logger.Verbosity.warn) #/< Log verbosity level + class TrackInfo: + track_id: int = int(0) + payload_type: int = int(0) + clock_rate: int = int(0) + channels: int = int(1) + media_type: str + encoding_name: str + control_path: str def __init__( self, - log_level: Logger.Verbosity = Logger.Verbosity(Logger.Verbosity.warn) + track_id: int = int(0), + payload_type: int = int(0), + clock_rate: int = int(0), + channels: int = int(1), + media_type: str = "", + encoding_name: str = "", + control_path: str = "" ) -> None: """Auto-generated default constructor with named params""" pass - def set_jpeg_frame_callback(self, cb: jpeg_frame_callback_t) -> None: - """/ Set callback for receiving complete JPEG frames. - / @param cb Callback receiving a shared pointer to the completed JpegFrame. - """ - pass - - def __init__(self) -> None: - """Auto-generated default constructor""" - pass - - -#################### #################### - -#################### #################### + class Config: + """/ Configuration for the RTSP client""" + server_address: str #/< The server IP Address to connect to + rtsp_port: int = int(8554) #/< The port of the RTSP server + path: str = str("/mjpeg/1") #/< The path to the RTSP stream on the server. Will be appended + #/< to the server address and port to form the full path of the + #/< form "rtsp://:" + #/ Generic frame callback for any codec (track_id, raw frame data) + on_frame: frame_callback_t = frame_callback_t(None) + #/ JPEG-specific frame callback (backward compatible). + #/ If set and no depacketizer is registered for PT 26, an MjpegDepacketizer + #/ is automatically created. + on_jpeg_frame: jpeg_frame_callback_t = jpeg_frame_callback_t(None) -class RtpDepacketizer: - """/ Abstract base class for reassembling media frames from incoming RTP packets. - / Concrete depacketizers (e.g. MJPEG, H.264) override process_packet() to - / accumulate payload data and invoke the frame callback when a complete frame - / has been assembled. - """ + #/ Called once if the client loses the server after playback starts. + #/ This callback is intended for applications that want to stop playback + #/ and re-enter service discovery or reconnect logic automatically. + on_connection_lost: disconnect_callback_t = disconnect_callback_t(None) - class Config: - """/ Configuration for RtpDepacketizer.""" - log_level: Logger.Verbosity = Logger.Verbosity(Logger.Verbosity.warn) #/< Log verbosity level + log_level: Logger.Verbosity = Logger.Verbosity.info #/< The verbosity of the logger def __init__( self, - log_level: Logger.Verbosity = Logger.Verbosity(Logger.Verbosity.warn) + server_address: str = "", + rtsp_port: int = int(8554), + path: str = str("/mjpeg/1"), + on_frame: frame_callback_t = frame_callback_t(None), + on_jpeg_frame: jpeg_frame_callback_t = jpeg_frame_callback_t(None), + on_connection_lost: disconnect_callback_t = disconnect_callback_t(None), + log_level: Logger.Verbosity = Logger.Verbosity.info ) -> None: """Auto-generated default constructor with named params""" pass - def process_packet(self, packet: RtpPacket) -> None: - """/ Process an incoming RTP packet, accumulating payload data. - / When a complete frame is assembled the frame callback is invoked. - / @param packet The RTP packet to process. + def send_request( + self, + method: str, + path: str, + extra_headers: std.unordered_map[str, str], + ec: std.error_code + ) -> str: + """/ Send an RTSP request to the server + / \note This is a blocking call + / \note This will parse the response and set the session ID if it is + / present in the response. If the response is not a 200 OK, then + / an error code will be set and the response will be returned. + / If the response is a 200 OK, then the response will be returned + / and the error code will be set to success. + / \param method The method to use for connecting. + / Options are "OPTIONS", "DESCRIBE", "SETUP", "PLAY", and "TEARDOWN" + / \param path The path to the RTSP stream on the server. + / \param extra_headers Any extra headers to send with the request. These + / will be added to the request after the CSeq and Session headers. The + / key is the header name and the value is the header value. For example, + / {"Accept": "application/sdp"} will add "Accept: application/sdp" to the + / request. The "User-Agent" header will be added automatically. The + / "CSeq" and "Session" headers will be added automatically. + / The "Accept" header will be added automatically. The "Transport" + / header will be added automatically for the "SETUP" method. Defaults to + / an empty map. + / \param ec The error code to set if an error occurs + / \return The response from the server """ pass - def set_frame_callback(self, cb: frame_callback_t) -> None: - """/ Set the callback for completed frames. - / @param cb The callback to invoke when a full frame is ready. + def connect(self, ec: std.error_code) -> None: + """/ Connect to the RTSP server + / Connects to the RTSP server and sends the OPTIONS request. + / \param ec The error code to set if an error occurs """ pass - def __init__(self) -> None: - """Auto-generated default constructor""" + def disconnect(self, ec: std.error_code) -> None: + """/ Disconnect from the RTSP server + / Disconnects from the RTSP server and sends the TEARDOWN request. + / \param ec The error code to set if an error occurs + """ pass + def describe(self, ec: std.error_code) -> None: + """/ Describe the RTSP stream + / Sends the DESCRIBE request to the RTSP server and parses the response. + / \param ec The error code to set if an error occurs + """ + pass -#################### #################### - - -#################### #################### - - - - -class GenericPacketizer: - """/ A generic RTP packetizer suitable for audio codecs (PCM, G.711, Opus, etc.) - / or any pre-formatted data that simply needs MTU-based chunking. It splits - / frame data into chunks of at most max_payload_size bytes and marks the last - / chunk with the RTP marker bit. - / - / \section generic_packetizer_ex1 Example - / \snippet generic_packetizer_example.cpp generic_packetizer example - """ - class Config: - """/ Configuration for GenericPacketizer.""" - max_payload_size: int = int(1400) #/< Maximum payload bytes per RTP packet - payload_type: int = int(96) #/< RTP payload type number - clock_rate: int = int(48000) #/< Clock rate in Hz for RTP timestamps - encoding_name: str = str("L16") #/< Encoding name for SDP rtpmap line - channels: int = int(1) #/< Number of audio channels - fmtp: str #/< Optional format parameters for SDP fmtp line - media_type: MediaType = MediaType(MediaType.audio) #/< Media type for the SDP m= line - log_level: Logger.Verbosity = Logger.Verbosity(Logger.Verbosity.warn) #/< Log verbosity level - def __init__( - self, - max_payload_size: int = int(1400), - payload_type: int = int(96), - clock_rate: int = int(48000), - encoding_name: str = str("L16"), - channels: int = int(1), - fmtp: str = "", - media_type: MediaType = MediaType(MediaType.audio), - log_level: Logger.Verbosity = Logger.Verbosity(Logger.Verbosity.warn) - ) -> None: - """Auto-generated default constructor with named params""" - pass - + @overload + def setup(self, ec: std.error_code) -> None: + """/ Setup the RTSP stream + / \note Starts the RTP and RTCP threads. + / Sends the SETUP request to the RTSP server and parses the response. + / \note The default ports are 5000 and 5001 for RTP and RTCP respectively. + / \note The default receive timeout is 5 seconds. + / \param ec The error code to set if an error occurs + """ + pass + @overload + def setup( + self, + rtp_port: int, + rtcp_port: int, + receive_timeout: std.chrono.duration[float], + ec: std.error_code + ) -> None: + """/ Setup the RTSP stream + / Sends the SETUP request to the RTSP server and parses the response. + / \note Starts the RTP and RTCP threads. + / \param rtp_port The RTP client port + / \param rtcp_port The RTCP client port + / \param receive_timeout The timeout for receiving RTP and RTCP packets + / \param ec The error code to set if an error occurs + """ + pass - def packetize(self, frame_data: std.span[ int]) -> List[RtpPayloadChunk]: - """/ Split frame data into RTP payload chunks of at most max_payload_size. - / The last (or only) chunk has its marker flag set. - / @param frame_data The raw frame bytes to packetize. - / @return A vector of RtpPayloadChunk ready to be wrapped in RTP packets. + def add_depacketizer(self, payload_type: int, depacketizer: RtpDepacketizer) -> None: + """/ Register a depacketizer for a specific RTP payload type. + / When RTP packets with this payload type are received, they are + / dispatched to the registered depacketizer. + / @param payload_type The RTP payload type (e.g., 26 for MJPEG, 96 for H264) + / @param depacketizer The depacketizer to handle packets of this type """ pass - def get_payload_type(self) -> int: - """/ Get the RTP payload type number. - / @return The configured RTP payload type. + def play(self, ec: std.error_code) -> None: + """/ Play the RTSP stream + / Sends the PLAY request to the RTSP server and parses the response. + / \param ec The error code to set if an error occurs """ pass - def get_clock_rate(self) -> int: - """/ Get the RTP clock rate. - / @return The configured clock rate in Hz. + def pause(self, ec: std.error_code) -> None: + """/ Pause the RTSP stream + / Sends the PAUSE request to the RTSP server and parses the response. + / \param ec The error code to set if an error occurs """ pass - def get_sdp_media_attributes(self) -> str: - """/ Generate the SDP media-level attribute lines for this codec. - / Produces an a=rtpmap line and, if fmtp is non-empty, an a=fmtp line. - / @return A string containing the SDP a= lines. + def teardown(self, ec: std.error_code) -> None: + """/ Teardown the RTSP stream + / Sends the TEARDOWN request to the RTSP server and parses the response. + / \param ec The error code to set if an error occurs """ pass - def get_sdp_media_line(self) -> str: - """/ Generate the SDP m= line for this codec. - / @return A string such as "m=audio 0 RTP/AVP 96". + def tracks(self) -> List[RtspClient.TrackInfo]: + """/ Get the parsed SDP track descriptions from the most recent DESCRIBE call. + / \return The ordered set of discovered media tracks. """ pass @@ -4697,109 +5257,151 @@ class GenericPacketizer: pass -#################### #################### +#################### #################### -#################### #################### +#################### #################### -class H264Packetizer: - """/ @brief RTP packetizer for H.264 video per RFC 6184. - / - / Accepts H.264 access units in Annex B byte-stream format (NAL units - / separated by 0x00000001 or 0x000001 start codes) and produces a sequence - / of RTP payload chunks suitable for transmission. - / - / Supports two NAL-unit packetization strategies: - / - **Single NAL unit mode** — NAL fits within max_payload_size. - / - **FU-A fragmentation** — NAL exceeds max_payload_size (packetization_mode >= 1). - / - / @note This class does not manage RTP headers (sequence numbers, timestamps, - / SSRC). The caller wraps each returned chunk into an RtpPacket. + + + +class RtspServer: + """/ Class for streaming MJPEG data from a camera using RTSP + RTP + / Starts a TCP socket to listen for RTSP connections, and then spawns off a + / new RTSP session for each connection. + / @see RtspSession + / @note This class does not currently send RTCP packets / - / \section h264_packetizer_ex1 Example - / \snippet h264_packetizer_example.cpp h264_packetizer example + / \section rtsp_server_ex1 RtspServer example + / \snippet rtsp_example.cpp rtsp_server_example """ class Config: - """/ Configuration for the H264Packetizer.""" - max_payload_size: int = int(1400) #/< Maximum payload bytes per RTP packet - payload_type: int = int(96) #/< Dynamic RTP payload type (typically 96–127). - profile_level_id: str #/< H.264 profile-level-id hex string, e.g. "42C01E". - packetization_mode: int = int(1) #/< 0 = single NAL only, 1 = non-interleaved (FU-A allowed). - sps: List[int] #/< Sequence Parameter Set raw bytes (without start code). - pps: List[int] #/< Picture Parameter Set raw bytes (without start code). - log_level: Logger.Verbosity = Logger.Verbosity(Logger.Verbosity.warn) #/< Log verbosity level + """/ @brief Configuration for the RTSP server""" + + server_address: str #/< The ip address of the server + port: int #/< The port to listen on + path: str #/< The path to the RTSP stream + max_data_size: int = 1000 #/< The maximum size of RTP packet data for the MJPEG stream. Frames will be broken + #/< up into multiple packets if they are larger than this. It seems that 1500 works + #/< well for sending, but is too large for the esp32 (camera-display) to receive + #/< properly. + log_level: Logger.Verbosity = Logger.Verbosity.warn #/< The log level for the RTSP server + accept_task_stack_size_bytes: int = default_accept_task_stack_size_bytes #/< RTSP accept-task stack size, in bytes + session_task_stack_size_bytes: int = default_session_task_stack_size_bytes #/< RTSP session-dispatch task stack size, in bytes + control_task_stack_size_bytes: int = RtspSession.Config.default_control_task_stack_size_bytes #/< Per-session RTSP + #/< control-task stack size, in + #/< bytes + def __init__( + self, + server_address: str = "", + port: int = int(), + path: str = "", + max_data_size: int = 1000, + log_level: Logger.Verbosity = Logger.Verbosity.warn, + accept_task_stack_size_bytes: int = default_accept_task_stack_size_bytes, + session_task_stack_size_bytes: int = default_session_task_stack_size_bytes, + control_task_stack_size_bytes: int = RtspSession.Config.default_control_task_stack_size_bytes + ) -> None: + """Auto-generated default constructor with named params""" + pass + + class TrackConfig: + """/ Configuration for a media track to be registered with the server""" + track_id: int = int(0) #/< Track identifier + packetizer: RtpPacketizer #/< Codec-specific packetizer def __init__( self, - max_payload_size: int = int(1400), - payload_type: int = int(96), - profile_level_id: str = "", - packetization_mode: int = int(1), - sps: List[int] = List[int](), - pps: List[int] = List[int](), - log_level: Logger.Verbosity = Logger.Verbosity(Logger.Verbosity.warn) + track_id: int = int(0), + packetizer: RtpPacketizer = RtpPacketizer() ) -> None: """Auto-generated default constructor with named params""" pass - def packetize(self, frame_data: std.span[ int]) -> List[RtpPayloadChunk]: - """/ Packetize a complete H.264 access unit (Annex B format). - / - / The input may contain multiple NAL units separated by 3-byte or 4-byte - / start codes. Each NAL is individually packetized (single NAL or FU-A). - / The marker bit is set on the last chunk of the last NAL unit in the - / access unit. - / - / @param frame_data Raw Annex B byte-stream of one access unit. - / @return Vector of RTP payload chunks ready for transmission. + def set_session_log_level(self, log_level: Logger.Verbosity) -> None: + """/ @brief Sets the log level for the RTSP sessions created by this server + / @note This does not affect the log level of the RTSP server itself + / @note This does not change the log level of any sessions that have + / already been created + / @param log_level The log level to set """ pass - def packetize_nal( + def start( self, - nal_data: std.span[ int], - is_last_nal: bool = True - ) -> List[RtpPayloadChunk]: - """/ Packetize a single pre-parsed NAL unit (no start code prefix). - / - / @param nal_data The raw NAL unit bytes (including NAL header byte). - / @param is_last_nal If True, the marker bit is set on the last chunk. - / @return Vector of RTP payload chunks for this NAL. + accept_timeout: std.chrono.duration[float] = std.chrono.seconds(5) + ) -> bool: + """/ @brief Start the RTSP server + / Starts the accept task, session task, and binds the RTSP socket + / @param accept_timeout The timeout for accepting new connections + / @return True if the server was started successfully, False otherwise """ pass - def set_sps_pps(self, sps: std.span[ int], pps: std.span[ int]) -> None: - """/ Update the SPS and PPS used for SDP generation. - / @param sps Sequence Parameter Set raw bytes. - / @param pps Picture Parameter Set raw bytes. + def stop(self) -> None: + """/ @brief Stop the FTP server + / Stops the accept task, session task, and closes the RTSP socket """ pass - def get_payload_type(self) -> int: - """/ Get the RTP payload type. - / @return The dynamic payload type configured for H.264. + def add_track(self, config: RtspServer.TrackConfig) -> None: + """/ @brief Register a media track with the server. + / Each track has its own packetizer, SSRC, and sequence number. + / @param config Track configuration including the packetizer. """ pass - def get_clock_rate(self) -> int: - """/ Get the RTP clock rate for H.264 video. - / @return 90000 (fixed for H.264). + def has_active_sessions(self) -> bool: + """/ @brief Returns True when at least one session is actively playing. + / @return True if an active RTSP session is ready to receive RTP packets. """ pass - def get_sdp_media_attributes(self) -> str: - """/ Get the SDP attribute lines for H.264. - / @return SDP a= lines (rtpmap and fmtp) without trailing CRLF. + def get_capture_cooldown(self) -> std.chrono.milliseconds: + """/ @brief Returns how long capture should wait before queueing another frame. + / @return Remaining RTP backpressure cooldown, or zero if sending may resume. """ pass - def get_sdp_media_line(self) -> str: - """/ Get the SDP m= media line for H.264. - / @return SDP m= line without trailing CRLF. + def get_recommended_capture_period(self) -> std.chrono.milliseconds: + """/ @brief Returns the minimum recommended period between captured frames. + / @return Recommended capture period based on recent RTP backpressure history. + """ + pass + + @overload + def send_frame(self, track_id: int, frame_data: std.span[ int]) -> None: + """/ @brief Send a frame on a specific track. + / The track's packetizer splits the frame into RTP payload chunks, + / which are then wrapped with RTP headers and queued for delivery. + / @note Overwrites any existing pending packets for this track. + / @param track_id The track to send on. + / @param frame_data Raw encoded frame data. + """ + pass + + @overload + def send_frame(self, frame: JpegFrame) -> None: + """/ @brief Send a JPEG frame over the RTSP connection (backward compatible). + / If no tracks have been added, lazily creates a default MJPEG track on + / track 0. Uses the legacy RtpJpegPacket packetization to preserve the + / exact wire format for existing MJPEG users. + / @note Overwrites any existing frame that has not been sent. + / @param frame The frame to send. + """ + pass + + @overload + def send_frame(self, frame_data: std.span[ int]) -> None: + """/ @brief Send raw JPEG bytes over the default MJPEG track. + / Uses the legacy MJPEG RTP packetization path without copying the frame + / into an intermediate JpegFrame object. + / @note Overwrites any existing frame that has not been sent. + / @param frame_data Complete JPEG bytes, including header and EOI marker. """ pass @@ -4807,118 +5409,153 @@ class H264Packetizer: """Auto-generated default constructor""" pass +#################### #################### -#################### #################### +#################### #################### -#################### #################### -class MjpegPacketizer: - """/ MJPEG packetizer that fragments JPEG frames into RFC 2435 RTP payloads. - / - / This class takes complete JPEG frames and produces RTP payload chunks - / suitable for MJPEG streaming. Each chunk contains an RFC 2435 MJPEG - / header, and the first chunk additionally includes quantization tables. + + +class RtspSession: + """/ Class that reepresents an RTSP session, which is uniquely identified by a + / session id and sends frame data over RTP and RTCP to the client """ + class Track: + """/ Represents one media track within an RTSP session""" + track_id: int = int(0) #/< Track identifier (matches trackID=N in SDP) + control_path: str #/< Control path suffix (e.g., "trackID=0") + rtp_socket: UdpSocket #/< RTP socket for this track + rtcp_socket: UdpSocket #/< RTCP socket for this track + client_rtp_port: int = int(0) #/< Client's RTP port + client_rtcp_port: int = int(0) #/< Client's RTCP port + setup_complete: bool = bool(False) #/< Whether SETUP has been completed for this track + + def __init__(self) -> None: + pass + class Config: - """/ Configuration for the MJPEG packetizer.""" - max_payload_size: int = int(1400) #/< Maximum payload bytes per RTP packet - log_level: Logger.Verbosity = Logger.Verbosity(Logger.Verbosity.warn) #/< Log verbosity level + """/ Configuration for the RTSP session""" + + server_address: str #/< The address of the server + rtsp_path: str #/< The RTSP path of the session + receive_timeout: std.chrono.duration[float] = std.chrono.seconds(5) #/< The timeout for receiving data. Should be > 0. + control_task_stack_size_bytes: int = default_control_task_stack_size_bytes #/< RTSP control-task stack size, in bytes + #/ SDP generator callback. If set, called during DESCRIBE to produce the SDP body. + #/ If not set, a default MJPEG SDP is generated for backward compatibility. + #/ @param session_path Full RTSP path (e.g., "rtsp://ip:port/path") + #/ @param session_id The session ID + #/ @param server_address The server address with port + sdp_generator: std.function[str( str session_path, int session_id, str server_address)] + log_level: Logger.Verbosity = Logger.Verbosity.warn #/< The log level of the session def __init__( self, - max_payload_size: int = int(1400), - log_level: Logger.Verbosity = Logger.Verbosity(Logger.Verbosity.warn) + server_address: str = "", + rtsp_path: str = "", + receive_timeout: std.chrono.duration[float] = std.chrono.seconds(5), + control_task_stack_size_bytes: int = default_control_task_stack_size_bytes, + log_level: Logger.Verbosity = Logger.Verbosity.warn ) -> None: """Auto-generated default constructor with named params""" pass - def get_payload_type(self) -> int: - """/ Get the RTP payload type for MJPEG. - / @return 26 (static JPEG payload type). + + def get_session_id(self) -> int: + """/ @brief Get the session id + / @return The session id """ pass - def get_clock_rate(self) -> int: - """/ Get the RTP clock rate for MJPEG. - / @return 90000 Hz. + def is_closed(self) -> bool: + """/ @brief Check if the session is closed + / @return True if the session is closed, False otherwise """ pass - def get_sdp_media_attributes(self) -> str: - """/ Get the SDP media attributes for MJPEG. - / @return SDP rtpmap attribute string. + def is_connected(self) -> bool: + """/ Get whether the session is connected + / @return True if the session is connected, False otherwise """ pass - def get_sdp_media_line(self) -> str: - """/ Get the SDP media line for MJPEG. - / @return SDP media description line. + def is_active(self) -> bool: + """/ Get whether the session is active + / @return True if the session is active, False otherwise """ pass - def __init__(self) -> None: - """Auto-generated default constructor""" - pass - - -#################### #################### - - -#################### #################### - - + def play(self) -> None: + """/ Mark the session as active + / This will cause the server to start sending frames to the client + """ + pass -class RtpPacketizer: - """/ Abstract base class for splitting media frames into RTP payload chunks. - / Concrete packetizers (e.g. MJPEG, H.264) override the pure-virtual methods - / to produce codec-specific payloads. The RTSP server wraps each returned - / RtpPayloadChunk with an RTP header before sending. - """ - class Config: - """/ Configuration for RtpPacketizer.""" - max_payload_size: int = int(1400) #/< Maximum payload bytes per RTP packet - log_level: Logger.Verbosity = Logger.Verbosity(Logger.Verbosity.warn) #/< Log verbosity level - def __init__( - self, - max_payload_size: int = int(1400), - log_level: Logger.Verbosity = Logger.Verbosity(Logger.Verbosity.warn) - ) -> None: - """Auto-generated default constructor with named params""" - pass + def pause(self) -> None: + """/ Pause the session + / This will cause the server to stop sending frames to the client + / @note This does not stop the session, it just pauses it + / @note This is useful for when the client is buffering + """ + pass + def teardown(self) -> None: + """/ Teardown the session + / This will cause the server to stop sending frames to the client + / and close the connection + """ + pass + @overload + def send_rtp_packet(self, track_id: int, packet: RtpPacket) -> bool: + """/ Send an RTP packet on a specific track + / @param track_id The track to send on + / @param packet The RTP packet to send + / @return True if the packet was sent successfully, False otherwise + """ + pass - def packetize(self, frame_data: std.span[ int]) -> List[RtpPayloadChunk]: - """/ Packetize a complete media frame into RTP payload chunks. - / @param frame_data The raw frame bytes to packetize. - / @return A vector of RtpPayloadChunk ready to be wrapped in RTP packets. + @overload + def send_rtp_packet(self, track_id: int, packet_data: std.span[ int]) -> bool: + """/ Send a serialized RTP packet on a specific track. + / @param track_id The track to send on + / @param packet_data Serialized RTP packet bytes + / @return True if the packet was sent successfully, False otherwise """ pass - def get_payload_type(self) -> int: - """/ Get the RTP payload type number for this codec. - / @return The RTP payload type (e.g. 26 for MJPEG, 96 for dynamic). + @overload + def send_rtp_packet(self, packet: RtpPacket) -> bool: + """/ Send an RTP packet to the client (backward compat — sends on default track 0) + / @param packet The RTP packet to send + / @return True if the packet was sent successfully, False otherwise """ pass - def get_clock_rate(self) -> int: - """/ Get the RTP clock rate for timestamp calculation. - / @return The clock rate in Hz (e.g. 90000 for video, 8000 for audio). + @overload + def send_rtp_packet(self, packet_data: std.span[ int]) -> bool: + """/ Send a serialized RTP packet to the client (default track 0). + / @param packet_data Serialized RTP packet bytes + / @return True if the packet was sent successfully, False otherwise """ pass - def get_sdp_media_attributes(self) -> str: - """/ Generate the SDP media-level attributes for this codec. - / @return A string containing SDP a= lines (without trailing CRLF). + @overload + def send_rtcp_packet(self, track_id: int, packet: RtcpPacket) -> bool: + """/ Send an RTCP packet on a specific track + / @param track_id The track to send on + / @param packet The RTCP packet to send + / @return True if the packet was sent successfully, False otherwise """ pass - def get_sdp_media_line(self) -> str: - """/ Generate the SDP m= line for this codec. - / @return A string containing the SDP m= line (without trailing CRLF). + @overload + def send_rtcp_packet(self, packet: RtcpPacket) -> bool: + """/ Send an RTCP packet to the client (backward compat — sends on default track 0) + / @param packet The RTCP packet to send + / @return True if the packet was sent successfully, False otherwise """ pass @@ -4926,8 +5563,7 @@ class RtpPacketizer: """Auto-generated default constructor""" pass - -#################### #################### +#################### #################### #################### #################### diff --git a/lib/python_bindings/module.cpp b/lib/python_bindings/module.cpp index efc2be1ff..a1cf28636 100644 --- a/lib/python_bindings/module.cpp +++ b/lib/python_bindings/module.cpp @@ -6,6 +6,11 @@ namespace py = pybind11; void py_init_module_espp(py::module &m); +// Hand-written bindings for the `cdr` and `rtps` components (see *_bindings.cpp for why they are +// not generated). Both must run after py_init_module_espp so shared types (e.g. Logger::Verbosity) +// and the module's classes are already registered. +void py_init_cdr(py::module &m); +void py_init_rtps(py::module &m); // This builds the native python module `espp` // it will be wrapped in a standard python module `espp` @@ -17,4 +22,6 @@ PYBIND11_MODULE(espp, m) { #endif py_init_module_espp(m); + py_init_cdr(m); + py_init_rtps(m); } diff --git a/lib/python_bindings/pybind_espp.cpp b/lib/python_bindings/pybind_espp.cpp index 6e74b5e01..9526bffba 100644 --- a/lib/python_bindings/pybind_espp.cpp +++ b/lib/python_bindings/pybind_espp.cpp @@ -72,10 +72,10 @@ void py_init_module_espp(py::module &m) { m, "Cobs", py::dynamic_attr(), "*\n * @brief COBS (Consistent Overhead Byte Stuffing) encoder/decoder\n *\n * Provides " "single-packet encoding and decoding using the COBS algorithm\n * with 0 as the " - "delimiter.\n * COBS encoding can add at most ⌈n/254⌉ + 1 bytes overhead. Plus 1 byte " - "for the delimiter\n * COBS changes the size of the packet by at least 1 byte, so it's " - "not possible to encode in\n * place. MAX_BLOCK_SIZE = 254 is the maximum number of " - "non-zero bytes in an encoded block.\n *\n * @see " + "delimiter.\n * COBS encoding can add at most ceil(n/254) + 1 bytes overhead, plus 1 " + "byte\n * for the delimiter.\n * COBS changes the size of the packet by at least 1 byte, " + "so it's not possible to encode in\n * place. MAX_BLOCK_SIZE = 254 is the maximum number " + "of non-zero bytes in an encoded block.\n *\n * @see " "https://en.wikipedia.org/wiki/Consistent_Overhead_Byte_Stuffing\n") .def(py::init<>()) // implicit default constructor .def_static("max_encoded_size", &espp::Cobs::max_encoded_size, py::arg("payload_len"), @@ -188,9 +188,9 @@ void py_init_module_espp(py::module &m) { auto pyClassRgb = py::class_(m, "Rgb", py::dynamic_attr(), "*\n * @brief Class representing a color using RGB color space.\n") - .def_readwrite("r", &espp::Rgb::r, "/< Red value ∈ [0, 1]") - .def_readwrite("g", &espp::Rgb::g, "/< Green value ∈ [0, 1]") - .def_readwrite("b", &espp::Rgb::b, "/< Blue value ∈ [0, 1]") + .def_readwrite("r", &espp::Rgb::r, "/< Red value in [0, 1]") + .def_readwrite("g", &espp::Rgb::g, "/< Green value in [0, 1]") + .def_readwrite("b", &espp::Rgb::b, "/< Blue value in [0, 1]") .def(py::init<>()) .def(py::init(), py::arg("r"), py::arg("g"), py::arg("b"), @@ -236,9 +236,9 @@ void py_init_module_espp(py::module &m) { auto pyClassHsv = py::class_(m, "Hsv", py::dynamic_attr(), "*\n * @brief Class representing a color using HSV color space.\n") - .def_readwrite("h", &espp::Hsv::h, "/< Hue ∈ [0, 360]") - .def_readwrite("s", &espp::Hsv::s, "/< Saturation ∈ [0, 1]") - .def_readwrite("v", &espp::Hsv::v, "/< Value ∈ [0, 1]") + .def_readwrite("h", &espp::Hsv::h, "/< Hue in [0, 360]") + .def_readwrite("s", &espp::Hsv::s, "/< Saturation in [0, 1]") + .def_readwrite("v", &espp::Hsv::v, "/< Value in [0, 1]") .def(py::init<>()) .def(py::init(), py::arg("h"), py::arg("s"), py::arg("v"), @@ -650,15 +650,16 @@ void py_init_module_espp(py::module &m) { "mapping to the output range - this will mean\n * that a values within the ranges " "[minimum, minimum+deadband] and\n * [maximum-deadband, maximum] will all map to the " "output_center and\n * the input center will map to both output_max and output_min\n " - "* depending on the sign of the input.\n *\n * @note When inverting the input range, " - "you are introducing a discontinuity\n * between the input distribution and the output " - "distribution at the\n * input center. Noise around the input's center value will " - "create\n * oscillations in the output which will jump between output maximum\n * " - " and output minimum. Therefore it is advised to use \\p invert_input\n * sparignly, " - "and to set the values robustly.\n *\n * The RangeMapper can be optionally configured " - "to invert the output,\n * so that after converting from the input range to the " - "output range,\n * it will flip the sign on the output.\n *\n * \\section " - "range_mapper_ex1 Example\n * \\snippet math_example.cpp range_mapper example\n"); + "* depending on the sign of the input.\n *\n * @tparam T Numeric type to use for the " + "input and output values.\n *\n * @note When inverting the input range, you are introducing " + "a discontinuity\n * between the input distribution and the output distribution at " + "the\n * input center. Noise around the input's center value will create\n * " + "oscillations in the output which will jump between output maximum\n * and output " + "minimum. Therefore it is advised to use \\p invert_input\n * sparignly, and to set " + "the values robustly.\n *\n * The RangeMapper can be optionally configured to invert " + "the output,\n * so that after converting from the input range to the output range,\n " + "* it will flip the sign on the output.\n *\n * \\section range_mapper_ex1 Example\n " + "* \\snippet math_example.cpp range_mapper example\n"); { // inner classes & enums of RangeMapper_int auto pyClassRangeMapper_ClassConfig = @@ -670,16 +671,16 @@ void py_init_module_espp(py::module &m) { .def(py::init<>([](int center = int(), int center_deadband = 0, int minimum = int(), int maximum = int(), int range_deadband = 0, int output_center = 0, int output_range = 1, bool invert_output = false) { - auto r = std::make_unique::Config>(); - r->center = center; - r->center_deadband = center_deadband; - r->minimum = minimum; - r->maximum = maximum; - r->range_deadband = range_deadband; - r->output_center = output_center; - r->output_range = output_range; - r->invert_output = invert_output; - return r; + auto r_ctor_ = std::make_unique::Config>(); + r_ctor_->center = center; + r_ctor_->center_deadband = center_deadband; + r_ctor_->minimum = minimum; + r_ctor_->maximum = maximum; + r_ctor_->range_deadband = range_deadband; + r_ctor_->output_center = output_center; + r_ctor_->output_range = output_range; + r_ctor_->invert_output = invert_output; + return r_ctor_; }), py::arg("center") = int(), py::arg("center_deadband") = 0, py::arg("minimum") = int(), py::arg("maximum") = int(), @@ -771,15 +772,16 @@ void py_init_module_espp(py::module &m) { "mapping to the output range - this will mean\n * that a values within the ranges " "[minimum, minimum+deadband] and\n * [maximum-deadband, maximum] will all map to the " "output_center and\n * the input center will map to both output_max and output_min\n " - "* depending on the sign of the input.\n *\n * @note When inverting the input range, " - "you are introducing a discontinuity\n * between the input distribution and the output " - "distribution at the\n * input center. Noise around the input's center value will " - "create\n * oscillations in the output which will jump between output maximum\n * " - " and output minimum. Therefore it is advised to use \\p invert_input\n * sparignly, " - "and to set the values robustly.\n *\n * The RangeMapper can be optionally configured " - "to invert the output,\n * so that after converting from the input range to the " - "output range,\n * it will flip the sign on the output.\n *\n * \\section " - "range_mapper_ex1 Example\n * \\snippet math_example.cpp range_mapper example\n"); + "* depending on the sign of the input.\n *\n * @tparam T Numeric type to use for the " + "input and output values.\n *\n * @note When inverting the input range, you are introducing " + "a discontinuity\n * between the input distribution and the output distribution at " + "the\n * input center. Noise around the input's center value will create\n * " + "oscillations in the output which will jump between output maximum\n * and output " + "minimum. Therefore it is advised to use \\p invert_input\n * sparignly, and to set " + "the values robustly.\n *\n * The RangeMapper can be optionally configured to invert " + "the output,\n * so that after converting from the input range to the output range,\n " + "* it will flip the sign on the output.\n *\n * \\section range_mapper_ex1 Example\n " + "* \\snippet math_example.cpp range_mapper example\n"); { // inner classes & enums of RangeMapper_float auto pyClassRangeMapper_ClassConfig = @@ -792,16 +794,16 @@ void py_init_module_espp(py::module &m) { float minimum = float(), float maximum = float(), float range_deadband = 0, float output_center = 0, float output_range = 1, bool invert_output = false) { - auto r = std::make_unique::Config>(); - r->center = center; - r->center_deadband = center_deadband; - r->minimum = minimum; - r->maximum = maximum; - r->range_deadband = range_deadband; - r->output_center = output_center; - r->output_range = output_range; - r->invert_output = invert_output; - return r; + auto r_ctor_ = std::make_unique::Config>(); + r_ctor_->center = center; + r_ctor_->center_deadband = center_deadband; + r_ctor_->minimum = minimum; + r_ctor_->maximum = maximum; + r_ctor_->range_deadband = range_deadband; + r_ctor_->output_center = output_center; + r_ctor_->output_range = output_range; + r_ctor_->invert_output = invert_output; + return r_ctor_; }), py::arg("center") = float(), py::arg("center_deadband") = 0, py::arg("minimum") = float(), py::arg("maximum") = float(), @@ -881,7 +883,7 @@ void py_init_module_espp(py::module &m) { "distribution.\n") .def("unmap", &espp::RangeMapper::unmap, py::arg("v"), "*\n * @brief Unmap a value \\p v from the configured output range (centered,\n * " - " default [-1,1]) back into the input distribution.\n * @param T&v Value from the " + " default [-1,1]) back into the input distribution.\n * @param v Value from the " "centered output distribution.\n * @return Value within the input distribution.\n"); //////////////////// //////////////////// @@ -1585,17 +1587,6 @@ void py_init_module_espp(py::module &m) { } // end of inner classes & enums of Pid pyClassPid.def(py::init()) - .def("set_config", &espp::Pid::set_config, py::arg("config"), py::arg("reset_state") = true, - "*\n * @brief Change the gains and other configuration for the PID controller.\n * " - "@param config Configuration struct with new gains and sampling time.\n * @param " - "reset_state Reset / clear the PID controller state.\n") - .def("clear", &espp::Pid::clear, "*\n * @brief Clear the PID controller state.\n") - .def("update", &espp::Pid::update, py::arg("error"), - "*\n * @brief Update the PID controller with the latest error measurement,\n * " - " getting the output control signal in return.\n *\n * @note Tracks invocation " - "timing to better compute time-accurate\n * integral/derivative signals.\n " - "*\n * @param error Latest error signal.\n * @return The output control signal " - "based on the PID state and error.\n") .def("__call__", &espp::Pid::operator(), py::arg("error"), "*\n * @brief Update the PID controller with the latest error measurement,\n * " " getting the output control signal in return.\n *\n * @note Tracks invocation " @@ -1966,9 +1957,9 @@ void py_init_module_espp(py::module &m) { py::class_(pyClassUdpSocket, "Config", py::dynamic_attr(), "") .def( py::init<>([](espp::Logger::Verbosity log_level = {espp::Logger::Verbosity::WARN}) { - auto r = std::make_unique(); - r->log_level = log_level; - return r; + auto r_ctor_ = std::make_unique(); + r_ctor_->log_level = log_level; + return r_ctor_; }), py::arg("log_level") = espp::Logger::Verbosity{espp::Logger::Verbosity::WARN}) .def_readwrite("log_level", &espp::UdpSocket::Config::log_level, @@ -1976,6 +1967,8 @@ void py_init_module_espp(py::module &m) { } // end of inner classes & enums of UdpSocket pyClassUdpSocket.def(py::init()) + .def("stop_receiving", &espp::UdpSocket::stop_receiving, + "/ Stop the receive task, if one is running, and close the socket.") .def("send", py::overload_cast &, const espp::UdpSocket::SendConfig &>( &espp::UdpSocket::send), @@ -2108,11 +2101,6 @@ void py_init_module_espp(py::module &m) { .def(py::init()) .def(py::init()) - .def_static("make_unique", &espp::Task::make_unique, py::arg("config"), - "*\n * @brief Get a unique pointer to a new task created with \\p config.\n " - "* Useful to not have to use templated std::make_unique (less typing).\n " - " * @param config Config struct to initialize the Task with.\n * @return " - "std::unique_ptr pointer to the newly created task.\n") .def("start", &espp::Task::start, "*\n * @brief Start executing the task.\n *\n * @return True if the task started, " "False if it was already started.\n") @@ -2516,117 +2504,6 @@ void py_init_module_espp(py::module &m) { "full frame is ready."); //////////////////// //////////////////// - //////////////////// //////////////////// - auto pyClassGenericDepacketizer = py::class_>( - m, "GenericDepacketizer", py::dynamic_attr(), - "/ A generic RTP depacketizer that reassembles media frames from incoming RTP\n/ packets. It " - "accumulates payload data until a packet with the marker bit set\n/ is received, then " - "delivers the complete frame via the frame callback. If a\n/ packet arrives with a different " - "RTP timestamp than the current accumulation\n/ buffer, the old buffer is discarded and a " - "new one is started.\n/\n/ This is suitable for audio codecs (PCM, G.711, Opus, etc.) or any " - "payload\n/ format that uses simple marker-based framing.\n/\n/ \\section " - "generic_depacketizer_ex1 Example\n/ \\snippet generic_depacketizer_example.cpp " - "generic_depacketizer example"); - - { // inner classes & enums of GenericDepacketizer - auto pyClassGenericDepacketizer_ClassConfig = - py::class_(pyClassGenericDepacketizer, "Config", - py::dynamic_attr(), - "/ Configuration for GenericDepacketizer.") - .def( - py::init<>([](espp::Logger::Verbosity log_level = {espp::Logger::Verbosity::WARN}) { - auto r_ctor_ = std::make_unique(); - r_ctor_->log_level = log_level; - return r_ctor_; - }), - py::arg("log_level") = espp::Logger::Verbosity{espp::Logger::Verbosity::WARN}) - .def_readwrite("log_level", &espp::GenericDepacketizer::Config::log_level, - "/< Log verbosity level"); - } // end of inner classes & enums of GenericDepacketizer - - pyClassGenericDepacketizer - .def(py::init(), - py::arg("config") = espp::GenericDepacketizer::Config{}) - .def("process_packet", &espp::GenericDepacketizer::process_packet, py::arg("packet"), - "/ Process an incoming RTP packet.\n/ Payload data is accumulated until a packet with " - "the marker bit set is\n/ received. At that point the assembled frame is delivered via " - "the frame\n/ callback and the buffer is reset.\n/ @param packet The RTP packet to " - "process."); - //////////////////// //////////////////// - - //////////////////// //////////////////// - auto pyClassH264Depacketizer = py::class_>( - m, "H264Depacketizer", py::dynamic_attr(), - "/ @brief RTP depacketizer for H.264 video per RFC 6184.\n/\n/ Reassembles H.264 access " - "units from incoming RTP packets. Supports:\n/ - **Single NAL unit** packets (NAL type " - "1–23)\n/ - **STAP-A** aggregation packets (NAL type 24)\n/ - **FU-A** fragmentation " - "packets (NAL type 28)\n/\n/ When the RTP marker bit is set, the accumulated NAL units are " - "delivered\n/ as one Annex B byte-stream (each NAL prefixed with 0x00 0x00 0x00 0x01)\n/ via " - "the frame callback set with set_frame_callback().\n/\n/ \\section h264_depacketizer_ex1 " - "Example\n/ \\snippet h264_depacketizer_example.cpp h264_depacketizer example"); - - { // inner classes & enums of H264Depacketizer - auto pyClassH264Depacketizer_ClassConfig = - py::class_(pyClassH264Depacketizer, "Config", - py::dynamic_attr(), - "/ Configuration for the H264Depacketizer.") - .def( - py::init<>([](espp::Logger::Verbosity log_level = {espp::Logger::Verbosity::WARN}) { - auto r_ctor_ = std::make_unique(); - r_ctor_->log_level = log_level; - return r_ctor_; - }), - py::arg("log_level") = espp::Logger::Verbosity{espp::Logger::Verbosity::WARN}) - .def_readwrite("log_level", &espp::H264Depacketizer::Config::log_level, - "/< Log verbosity level"); - } // end of inner classes & enums of H264Depacketizer - - pyClassH264Depacketizer - .def(py::init(), - py::arg("config") = espp::H264Depacketizer::Config{}) - .def("process_packet", &espp::H264Depacketizer::process_packet, py::arg("packet"), - "/ Process an incoming RTP packet containing H.264 payload.\n/\n/ Handles single NAL, " - "STAP-A, and FU-A packet types. NAL units are\n/ buffered until the RTP marker bit " - "indicates the end of an access unit,\n/ at which point the complete Annex B frame is " - "delivered via the callback.\n/\n/ @param packet The RTP packet to process."); - //////////////////// //////////////////// - - //////////////////// //////////////////// - auto pyClassMjpegDepacketizer = py::class_>( - m, "MjpegDepacketizer", py::dynamic_attr(), - "/ MJPEG depacketizer that reassembles JPEG frames from RTP packets.\n/\n/ This class " - "receives individual RTP packets containing RFC 2435 MJPEG\n/ payloads, reassembles the scan " - "data fragments, reconstructs the JPEG\n/ header from the MJPEG header fields, and delivers " - "complete JPEG frames\n/ through callbacks."); - - { // inner classes & enums of MjpegDepacketizer - auto pyClassMjpegDepacketizer_ClassConfig = - py::class_(pyClassMjpegDepacketizer, "Config", - py::dynamic_attr(), - "/ Configuration for the MJPEG depacketizer.") - .def( - py::init<>([](espp::Logger::Verbosity log_level = {espp::Logger::Verbosity::WARN}) { - auto r_ctor_ = std::make_unique(); - r_ctor_->log_level = log_level; - return r_ctor_; - }), - py::arg("log_level") = espp::Logger::Verbosity{espp::Logger::Verbosity::WARN}) - .def_readwrite("log_level", &espp::MjpegDepacketizer::Config::log_level, - "/< Log verbosity level"); - } // end of inner classes & enums of MjpegDepacketizer - - pyClassMjpegDepacketizer - .def(py::init(), - py::arg("config") = espp::MjpegDepacketizer::Config{}) - .def("set_jpeg_frame_callback", &espp::MjpegDepacketizer::set_jpeg_frame_callback, - py::arg("cb"), - "/ Set callback for receiving complete JPEG frames.\n/ @param cb Callback receiving a " - "shared pointer to the completed JpegFrame."); - //////////////////// //////////////////// - //////////////////// //////////////////// auto pyClassRtpPacketizer = py::class_>( m, "RtpPacketizer", py::dynamic_attr(), @@ -2674,6 +2551,42 @@ void py_init_module_espp(py::module &m) { "line (without trailing CRLF)."); //////////////////// //////////////////// + //////////////////// //////////////////// + auto pyClassGenericDepacketizer = py::class_>( + m, "GenericDepacketizer", py::dynamic_attr(), + "/ A generic RTP depacketizer that reassembles media frames from incoming RTP\n/ packets. It " + "accumulates payload data until a packet with the marker bit set\n/ is received, then " + "delivers the complete frame via the frame callback. If a\n/ packet arrives with a different " + "RTP timestamp than the current accumulation\n/ buffer, the old buffer is discarded and a " + "new one is started.\n/\n/ This is suitable for audio codecs (PCM, G.711, Opus, etc.) or any " + "payload\n/ format that uses simple marker-based framing.\n/\n/ \\section " + "generic_depacketizer_ex1 Example\n/ \\snippet rtsp_example.cpp generic_depacketizer_test"); + + { // inner classes & enums of GenericDepacketizer + auto pyClassGenericDepacketizer_ClassConfig = + py::class_(pyClassGenericDepacketizer, "Config", + py::dynamic_attr(), + "/ Configuration for GenericDepacketizer.") + .def( + py::init<>([](espp::Logger::Verbosity log_level = {espp::Logger::Verbosity::WARN}) { + auto r_ctor_ = std::make_unique(); + r_ctor_->log_level = log_level; + return r_ctor_; + }), + py::arg("log_level") = espp::Logger::Verbosity{espp::Logger::Verbosity::WARN}) + .def_readwrite("log_level", &espp::GenericDepacketizer::Config::log_level, + "/< Log verbosity level"); + } // end of inner classes & enums of GenericDepacketizer + + pyClassGenericDepacketizer.def(py::init()) + .def("process_packet", &espp::GenericDepacketizer::process_packet, py::arg("packet"), + "/ Process an incoming RTP packet.\n/ Payload data is accumulated until a packet with " + "the marker bit set is\n/ received. At that point the assembled frame is delivered via " + "the frame\n/ callback and the buffer is reset.\n/ @param packet The RTP packet to " + "process."); + //////////////////// //////////////////// + //////////////////// //////////////////// auto pyClassGenericPacketizer = py::class_>( @@ -2681,8 +2594,8 @@ void py_init_module_espp(py::module &m) { "/ A generic RTP packetizer suitable for audio codecs (PCM, G.711, Opus, etc.)\n/ or any " "pre-formatted data that simply needs MTU-based chunking. It splits\n/ frame data into " "chunks of at most max_payload_size bytes and marks the last\n/ chunk with the RTP marker " - "bit.\n/\n/ \\section generic_packetizer_ex1 Example\n/ \\snippet " - "generic_packetizer_example.cpp generic_packetizer example"); + "bit.\n/\n/ \\section generic_packetizer_ex1 Example\n/ \\snippet rtsp_example.cpp " + "generic_packetizer_test"); { // inner classes & enums of GenericPacketizer auto pyClassGenericPacketizer_ClassConfig = @@ -2730,9 +2643,7 @@ void py_init_module_espp(py::module &m) { "/< Log verbosity level"); } // end of inner classes & enums of GenericPacketizer - pyClassGenericPacketizer - .def(py::init(), - py::arg("config") = espp::GenericPacketizer::Config{}) + pyClassGenericPacketizer.def(py::init()) .def("packetize", &espp::GenericPacketizer::packetize, py::arg("frame_data"), "/ Split frame data into RTP payload chunks of at most max_payload_size.\n/ The last " "(or only) chunk has its marker flag set.\n/ @param frame_data The raw frame bytes to " @@ -2750,19 +2661,55 @@ void py_init_module_espp(py::module &m) { "RTP/AVP 96\"."); //////////////////// //////////////////// + //////////////////// //////////////////// + auto pyClassH264Depacketizer = py::class_>( + m, "H264Depacketizer", py::dynamic_attr(), + "/ @brief RTP depacketizer for H.264 video per RFC 6184.\n/\n/ Reassembles H.264 access " + "units from incoming RTP packets. Supports:\n/ - **Single NAL unit** packets (NAL type " + "1–23)\n/ - **STAP-A** aggregation packets (NAL type 24)\n/ - **FU-A** fragmentation " + "packets (NAL type 28)\n/\n/ When the RTP marker bit is set, the accumulated NAL units are " + "delivered\n/ as one Annex B byte-stream (each NAL prefixed with 0x00 0x00 0x00 0x01)\n/ via " + "the frame callback set with set_frame_callback().\n/\n/ \\section h264_depacketizer_ex1 " + "Example\n/ \\snippet rtsp_example.cpp h264_depacketizer_test"); + + { // inner classes & enums of H264Depacketizer + auto pyClassH264Depacketizer_ClassConfig = + py::class_(pyClassH264Depacketizer, "Config", + py::dynamic_attr(), + "/ Configuration for the H264Depacketizer.") + .def( + py::init<>([](espp::Logger::Verbosity log_level = {espp::Logger::Verbosity::WARN}) { + auto r_ctor_ = std::make_unique(); + r_ctor_->log_level = log_level; + return r_ctor_; + }), + py::arg("log_level") = espp::Logger::Verbosity{espp::Logger::Verbosity::WARN}) + .def_readwrite("log_level", &espp::H264Depacketizer::Config::log_level, + "/< Log verbosity level"); + } // end of inner classes & enums of H264Depacketizer + + pyClassH264Depacketizer.def(py::init()) + .def("process_packet", &espp::H264Depacketizer::process_packet, py::arg("packet"), + "/ Process an incoming RTP packet containing H.264 payload.\n/\n/ Handles single NAL, " + "STAP-A, and FU-A packet types. NAL units are\n/ buffered until the RTP marker bit " + "indicates the end of an access unit,\n/ at which point the complete Annex B frame is " + "delivered via the callback.\n/\n/ @param packet The RTP packet to process."); + //////////////////// //////////////////// + //////////////////// //////////////////// - auto pyClassH264Packetizer = py::class_>( - m, "H264Packetizer", py::dynamic_attr(), - "/ @brief RTP packetizer for H.264 video per RFC 6184.\n/\n/ Accepts H.264 access units in " - "Annex B byte-stream format (NAL units\n/ separated by 0x00000001 or 0x000001 start codes) " - "and produces a sequence\n/ of RTP payload chunks suitable for transmission.\n/\n/ Supports " - "two NAL-unit packetization strategies:\n/ - **Single NAL unit mode** — NAL fits within " - "max_payload_size.\n/ - **FU-A fragmentation** — NAL exceeds max_payload_size " - "(packetization_mode >= 1).\n/\n/ @note This class does not manage RTP headers (sequence " - "numbers, timestamps,\n/ SSRC). The caller wraps each returned chunk into an " - "RtpPacket.\n/\n/ \\section h264_packetizer_ex1 Example\n/ \\snippet " - "h264_packetizer_example.cpp h264_packetizer example"); + auto pyClassH264Packetizer = + py::class_>( + m, "H264Packetizer", py::dynamic_attr(), + "/ @brief RTP packetizer for H.264 video per RFC 6184.\n/\n/ Accepts H.264 access units " + "in Annex B byte-stream format (NAL units\n/ separated by 0x00000001 or 0x000001 start " + "codes) and produces a sequence\n/ of RTP payload chunks suitable for " + "transmission.\n/\n/ Supports two NAL-unit packetization strategies:\n/ - **Single NAL " + "unit mode** — NAL fits within max_payload_size.\n/ - **FU-A fragmentation** — NAL " + "exceeds max_payload_size (packetization_mode >= 1).\n/\n/ @note This class does not " + "manage RTP headers (sequence numbers, timestamps,\n/ SSRC). The caller wraps each " + "returned chunk into an RtpPacket.\n/\n/ \\section h264_packetizer_ex1 Example\n/ " + "\\snippet rtsp_example.cpp h264_packetizer_test"); { // inner classes & enums of H264Packetizer auto pyClassH264Packetizer_ClassConfig = @@ -2806,9 +2753,7 @@ void py_init_module_espp(py::module &m) { "/< Log verbosity level"); } // end of inner classes & enums of H264Packetizer - pyClassH264Packetizer - .def(py::init(), - py::arg("config") = espp::H264Packetizer::Config{}) + pyClassH264Packetizer.def(py::init()) .def("packetize", &espp::H264Packetizer::packetize, py::arg("frame_data"), "/ Packetize a complete H.264 access unit (Annex B format).\n/\n/ The input may contain " "multiple NAL units separated by 3-byte or 4-byte\n/ start codes. Each NAL is " @@ -2836,6 +2781,38 @@ void py_init_module_espp(py::module &m) { "/ Get the SDP m= media line for H.264.\n/ @return SDP m= line without trailing CRLF."); //////////////////// //////////////////// + //////////////////// //////////////////// + auto pyClassMjpegDepacketizer = py::class_>( + m, "MjpegDepacketizer", py::dynamic_attr(), + "/ MJPEG depacketizer that reassembles JPEG frames from RTP packets.\n/\n/ This class " + "receives individual RTP packets containing RFC 2435 MJPEG\n/ payloads, reassembles the scan " + "data fragments, reconstructs the JPEG\n/ header from the MJPEG header fields, and delivers " + "complete JPEG frames\n/ through callbacks."); + + { // inner classes & enums of MjpegDepacketizer + auto pyClassMjpegDepacketizer_ClassConfig = + py::class_(pyClassMjpegDepacketizer, "Config", + py::dynamic_attr(), + "/ Configuration for the MJPEG depacketizer.") + .def( + py::init<>([](espp::Logger::Verbosity log_level = {espp::Logger::Verbosity::WARN}) { + auto r_ctor_ = std::make_unique(); + r_ctor_->log_level = log_level; + return r_ctor_; + }), + py::arg("log_level") = espp::Logger::Verbosity{espp::Logger::Verbosity::WARN}) + .def_readwrite("log_level", &espp::MjpegDepacketizer::Config::log_level, + "/< Log verbosity level"); + } // end of inner classes & enums of MjpegDepacketizer + + pyClassMjpegDepacketizer.def(py::init()) + .def("set_jpeg_frame_callback", &espp::MjpegDepacketizer::set_jpeg_frame_callback, + py::arg("cb"), + "/ Set callback for receiving complete JPEG frames.\n/ @param cb Callback receiving a " + "shared pointer to the completed JpegFrame."); + //////////////////// //////////////////// + //////////////////// //////////////////// auto pyClassMjpegPacketizer = py::class_>( @@ -2866,9 +2843,7 @@ void py_init_module_espp(py::module &m) { "/< Log verbosity level"); } // end of inner classes & enums of MjpegPacketizer - pyClassMjpegPacketizer - .def(py::init(), - py::arg("config") = espp::MjpegPacketizer::Config{}) + pyClassMjpegPacketizer.def(py::init()) .def("get_payload_type", &espp::MjpegPacketizer::get_payload_type, "/ Get the RTP payload type for MJPEG.\n/ @return 26 (static JPEG payload type).") .def("get_clock_rate", &espp::MjpegPacketizer::get_clock_rate, @@ -2948,8 +2923,6 @@ void py_init_module_espp(py::module &m) { //////////////////// //////////////////// auto pyClassJpegFrame = - // NOTE: you must keep the `std::shared_ptr` argument here since we return - // these and pass them via callback functions py::class_>( m, "JpegFrame", py::dynamic_attr(), "/ A class that represents a complete JPEG frame.\n/\n/ This class is used to collect " @@ -3019,6 +2992,8 @@ void py_init_module_espp(py::module &m) { "/ Get the size of the JPEG header data.\n/ @return The size of the JPEG header " "data in bytes.\n/ @note This is the size of the serialized JPEG header, not the " "image size.") + .def("is_valid", &espp::JpegHeader::is_valid, + "/ Returns whether this header parsed or serialized successfully.") .def("get_data", &espp::JpegHeader::get_data, "/ Get the JPEG header data.\n/ @return The JPEG header data.") .def("get_quantization_table", &espp::JpegHeader::get_quantization_table, @@ -3128,44 +3103,57 @@ void py_init_module_espp(py::module &m) { { // inner classes & enums of RtspClient auto pyClassRtspClient_ClassTrackInfo = py::class_(pyClassRtspClient, "TrackInfo", py::dynamic_attr(), - "/ Parsed SDP metadata for a discovered RTSP track") - .def(py::init<>()) - .def_readwrite("track_id", &espp::RtspClient::TrackInfo::track_id, - "/< Track identifier (matches trackID=N in SDP)") - .def_readwrite("payload_type", &espp::RtspClient::TrackInfo::payload_type, - "/< RTP payload type") - .def_readwrite("clock_rate", &espp::RtspClient::TrackInfo::clock_rate, - "/< RTP clock rate in Hz") - .def_readwrite("channels", &espp::RtspClient::TrackInfo::channels, - "/< Number of audio channels") - .def_readwrite("media_type", &espp::RtspClient::TrackInfo::media_type, - "/< SDP media type (audio/video)") - .def_readwrite("encoding_name", &espp::RtspClient::TrackInfo::encoding_name, - "/< Codec / encoding name from a=rtpmap") - .def_readwrite("control_path", &espp::RtspClient::TrackInfo::control_path, - "/< Resolved control path used for SETUP"); - + "") + .def(py::init<>([](int track_id = {0}, int payload_type = {0}, int clock_rate = {0}, + int channels = {1}, std::string media_type = std::string(), + std::string encoding_name = std::string(), + std::string control_path = std::string()) { + auto r_ctor_ = std::make_unique(); + r_ctor_->track_id = track_id; + r_ctor_->payload_type = payload_type; + r_ctor_->clock_rate = clock_rate; + r_ctor_->channels = channels; + r_ctor_->media_type = media_type; + r_ctor_->encoding_name = encoding_name; + r_ctor_->control_path = control_path; + return r_ctor_; + }), + py::arg("track_id") = int{0}, py::arg("payload_type") = int{0}, + py::arg("clock_rate") = int{0}, py::arg("channels") = int{1}, + py::arg("media_type") = std::string(), py::arg("encoding_name") = std::string(), + py::arg("control_path") = std::string()) + .def_readwrite("track_id", &espp::RtspClient::TrackInfo::track_id, "") + .def_readwrite("payload_type", &espp::RtspClient::TrackInfo::payload_type, "") + .def_readwrite("clock_rate", &espp::RtspClient::TrackInfo::clock_rate, "") + .def_readwrite("channels", &espp::RtspClient::TrackInfo::channels, "") + .def_readwrite("media_type", &espp::RtspClient::TrackInfo::media_type, "") + .def_readwrite("encoding_name", &espp::RtspClient::TrackInfo::encoding_name, "") + .def_readwrite("control_path", &espp::RtspClient::TrackInfo::control_path, ""); auto pyClassRtspClient_ClassConfig = py::class_(pyClassRtspClient, "Config", py::dynamic_attr(), "/ Configuration for the RTSP client") - .def(py::init<>([](std::string server_address = std::string(), int rtsp_port = {8554}, - std::string path = {"/mjpeg/1"}, - espp::RtspClient::frame_callback_t on_frame = {nullptr}, - espp::RtspClient::jpeg_frame_callback_t on_jpeg_frame = {nullptr}, - espp::Logger::Verbosity log_level = espp::Logger::Verbosity::INFO) { - auto r_ctor_ = std::make_unique(); - r_ctor_->server_address = server_address; - r_ctor_->rtsp_port = rtsp_port; - r_ctor_->path = path; - r_ctor_->on_frame = on_frame; - r_ctor_->on_jpeg_frame = on_jpeg_frame; - r_ctor_->log_level = log_level; - return r_ctor_; - }), + .def(py::init<>( + [](std::string server_address = std::string(), int rtsp_port = {8554}, + std::string path = {"/mjpeg/1"}, + espp::RtspClient::frame_callback_t on_frame = {nullptr}, + espp::RtspClient::jpeg_frame_callback_t on_jpeg_frame = {nullptr}, + espp::RtspClient::disconnect_callback_t on_connection_lost = {nullptr}, + espp::Logger::Verbosity log_level = espp::Logger::Verbosity::INFO) { + auto r_ctor_ = std::make_unique(); + r_ctor_->server_address = server_address; + r_ctor_->rtsp_port = rtsp_port; + r_ctor_->path = path; + r_ctor_->on_frame = on_frame; + r_ctor_->on_jpeg_frame = on_jpeg_frame; + r_ctor_->on_connection_lost = on_connection_lost; + r_ctor_->log_level = log_level; + return r_ctor_; + }), py::arg("server_address") = std::string(), py::arg("rtsp_port") = int{8554}, py::arg("path") = std::string{"/mjpeg/1"}, py::arg("on_frame") = espp::RtspClient::frame_callback_t{nullptr}, py::arg("on_jpeg_frame") = espp::RtspClient::jpeg_frame_callback_t{nullptr}, + py::arg("on_connection_lost") = espp::RtspClient::disconnect_callback_t{nullptr}, py::arg("log_level") = espp::Logger::Verbosity::INFO) .def_readwrite("server_address", &espp::RtspClient::Config::server_address, "/< The server IP Address to connect to") @@ -3179,11 +3167,16 @@ void py_init_module_espp(py::module &m) { "/ JPEG-specific frame callback (backward compatible).\n/ If set and no " "depacketizer is registered for PT 26, an MjpegDepacketizer\n/ is " "automatically created.") + .def_readwrite( + "on_connection_lost", &espp::RtspClient::Config::on_connection_lost, + "/ Called once if the client loses the server after playback starts.\n/ This " + "callback is intended for applications that want to stop playback\n/ and re-enter " + "service discovery or reconnect logic automatically.") .def_readwrite("log_level", &espp::RtspClient::Config::log_level, "/< The verbosity of the logger"); } // end of inner classes & enums of RtspClient - pyClassRtspClient.def(py::init(), py::arg("config")) + pyClassRtspClient.def(py::init()) .def("send_request", &espp::RtspClient::send_request, py::arg("method"), py::arg("path"), py::arg("extra_headers"), py::arg("ec"), "/ Send an RTSP request to the server\n/ \note This is a blocking call\n/ \note This " @@ -3211,10 +3204,6 @@ void py_init_module_espp(py::module &m) { .def("describe", &espp::RtspClient::describe, py::arg("ec"), "/ Describe the RTSP stream\n/ Sends the DESCRIBE request to the RTSP server and parses " "the response.\n/ \\param ec The error code to set if an error occurs") - .def( - "tracks", [](const espp::RtspClient &self) { return self.tracks(); }, - "/ Get the parsed SDP track descriptions from the most recent DESCRIBE call.\n/ " - "\\return The ordered set of discovered media tracks") .def("setup", py::overload_cast(&espp::RtspClient::setup), py::arg("ec"), "/ Setup the RTSP stream\n/ \note Starts the RTP and RTCP threads.\n/ Sends the SETUP " "request to the RTSP server and parses the response.\n/ \note The default ports are " @@ -3243,7 +3232,10 @@ void py_init_module_espp(py::module &m) { "response.\n/ \\param ec The error code to set if an error occurs") .def("teardown", &espp::RtspClient::teardown, py::arg("ec"), "/ Teardown the RTSP stream\n/ Sends the TEARDOWN request to the RTSP server and parses " - "the response.\n/ \\param ec The error code to set if an error occurs"); + "the response.\n/ \\param ec The error code to set if an error occurs") + .def("tracks", &espp::RtspClient::tracks, + "/ Get the parsed SDP track descriptions from the most recent DESCRIBE call.\n/ " + "\\return The ordered set of discovered media tracks."); //////////////////// //////////////////// //////////////////// //////////////////// @@ -3259,20 +3251,36 @@ void py_init_module_espp(py::module &m) { auto pyClassRtspServer_ClassConfig = py::class_(pyClassRtspServer, "Config", py::dynamic_attr(), "/ @brief Configuration for the RTSP server") - .def(py::init<>([](std::string server_address = std::string(), int port = int(), - std::string path = std::string(), size_t max_data_size = 1000, - espp::Logger::Verbosity log_level = espp::Logger::Verbosity::WARN) { - auto r_ctor_ = std::make_unique(); - r_ctor_->server_address = server_address; - r_ctor_->port = port; - r_ctor_->path = path; - r_ctor_->max_data_size = max_data_size; - r_ctor_->log_level = log_level; - return r_ctor_; - }), + .def(py::init<>( + [](std::string server_address = std::string(), int port = int(), + std::string path = std::string(), size_t max_data_size = 1000, + espp::Logger::Verbosity log_level = espp::Logger::Verbosity::WARN, + size_t accept_task_stack_size_bytes = + espp::RtspServer::Config::default_accept_task_stack_size_bytes, + size_t session_task_stack_size_bytes = + espp::RtspServer::Config::default_session_task_stack_size_bytes, + size_t control_task_stack_size_bytes = + espp::RtspSession::Config::default_control_task_stack_size_bytes) { + auto r_ctor_ = std::make_unique(); + r_ctor_->server_address = server_address; + r_ctor_->port = port; + r_ctor_->path = path; + r_ctor_->max_data_size = max_data_size; + r_ctor_->log_level = log_level; + r_ctor_->accept_task_stack_size_bytes = accept_task_stack_size_bytes; + r_ctor_->session_task_stack_size_bytes = session_task_stack_size_bytes; + r_ctor_->control_task_stack_size_bytes = control_task_stack_size_bytes; + return r_ctor_; + }), py::arg("server_address") = std::string(), py::arg("port") = int(), py::arg("path") = std::string(), py::arg("max_data_size") = 1000, - py::arg("log_level") = espp::Logger::Verbosity::WARN) + py::arg("log_level") = espp::Logger::Verbosity::WARN, + py::arg("accept_task_stack_size_bytes") = + espp::RtspServer::Config::default_accept_task_stack_size_bytes, + py::arg("session_task_stack_size_bytes") = + espp::RtspServer::Config::default_session_task_stack_size_bytes, + py::arg("control_task_stack_size_bytes") = + espp::RtspSession::Config::default_control_task_stack_size_bytes) .def_readwrite("server_address", &espp::RtspServer::Config::server_address, "/< The ip address of the server") .def_readwrite("port", &espp::RtspServer::Config::port, "/< The port to listen on") @@ -3282,27 +3290,36 @@ void py_init_module_espp(py::module &m) { "/< The maximum size of RTP packet data for the MJPEG stream. Frames " "will be broken") .def_readwrite("log_level", &espp::RtspServer::Config::log_level, - "/< The log level for the RTSP server"); + "/< The log level for the RTSP server") + .def_readwrite("accept_task_stack_size_bytes", + &espp::RtspServer::Config::accept_task_stack_size_bytes, + "/< RTSP accept-task stack size, in bytes") + .def_readwrite("session_task_stack_size_bytes", + &espp::RtspServer::Config::session_task_stack_size_bytes, + "/< RTSP session-dispatch task stack size, in bytes") + .def_readwrite("control_task_stack_size_bytes", + &espp::RtspServer::Config::control_task_stack_size_bytes, + "/< Per-session RTSP"); auto pyClassRtspServer_ClassTrackConfig = py::class_( pyClassRtspServer, "TrackConfig", py::dynamic_attr(), "/ Configuration for a media track to be registered with the server") - .def(py::init<>([](int track_id = {0}, - std::shared_ptr packetizer = {nullptr}) { + .def(py::init<>([](int track_id = {0}, std::shared_ptr packetizer = + std::shared_ptr()) { auto r_ctor_ = std::make_unique(); r_ctor_->track_id = track_id; r_ctor_->packetizer = packetizer; return r_ctor_; }), py::arg("track_id") = int{0}, - py::arg("packetizer") = std::shared_ptr{nullptr}) + py::arg("packetizer") = std::shared_ptr()) .def_readwrite("track_id", &espp::RtspServer::TrackConfig::track_id, "/< Track identifier") .def_readwrite("packetizer", &espp::RtspServer::TrackConfig::packetizer, "/< Codec-specific packetizer"); } // end of inner classes & enums of RtspServer - pyClassRtspServer.def(py::init(), py::arg("config")) + pyClassRtspServer.def(py::init()) .def("set_session_log_level", &espp::RtspServer::set_session_log_level, py::arg("log_level"), "/ @brief Sets the log level for the RTSP sessions created by this server\n/ @note This " "does not affect the log level of the RTSP server itself\n/ @note This does not change " @@ -3319,6 +3336,15 @@ void py_init_module_espp(py::module &m) { "/ @brief Register a media track with the server.\n/ Each track has its own packetizer, " "SSRC, and sequence number.\n/ @param config Track configuration including the " "packetizer.") + .def("has_active_sessions", &espp::RtspServer::has_active_sessions, + "/ @brief Returns True when at least one session is actively playing.\n/ @return True " + "if an active RTSP session is ready to receive RTP packets.") + .def("get_capture_cooldown", &espp::RtspServer::get_capture_cooldown, + "/ @brief Returns how long capture should wait before queueing another frame.\n/ " + "@return Remaining RTP backpressure cooldown, or zero if sending may resume.") + .def("get_recommended_capture_period", &espp::RtspServer::get_recommended_capture_period, + "/ @brief Returns the minimum recommended period between captured frames.\n/ @return " + "Recommended capture period based on recent RTP backpressure history.") .def("send_frame", py::overload_cast>(&espp::RtspServer::send_frame), py::arg("track_id"), py::arg("frame_data"), @@ -3332,7 +3358,13 @@ void py_init_module_espp(py::module &m) { "tracks have been added, lazily creates a default MJPEG track on\n/ track 0. Uses the " "legacy RtpJpegPacket packetization to preserve the\n/ exact wire format for existing " "MJPEG users.\n/ @note Overwrites any existing frame that has not been sent.\n/ @param " - "frame The frame to send."); + "frame The frame to send.") + .def("send_frame", py::overload_cast>(&espp::RtspServer::send_frame), + py::arg("frame_data"), + "/ @brief Send raw JPEG bytes over the default MJPEG track.\n/ Uses the legacy MJPEG " + "RTP packetization path without copying the frame\n/ into an intermediate JpegFrame " + "object.\n/ @note Overwrites any existing frame that has not been sent.\n/ @param " + "frame_data Complete JPEG bytes, including header and EOI marker."); //////////////////// //////////////////// //////////////////// //////////////////// @@ -3367,16 +3399,21 @@ void py_init_module_espp(py::module &m) { [](std::string server_address = std::string(), std::string rtsp_path = std::string(), std::chrono::duration receive_timeout = std::chrono::seconds(5), + size_t control_task_stack_size_bytes = + espp::RtspSession::Config::default_control_task_stack_size_bytes, espp::Logger::Verbosity log_level = espp::Logger::Verbosity::WARN) { auto r_ctor_ = std::make_unique(); r_ctor_->server_address = server_address; r_ctor_->rtsp_path = rtsp_path; r_ctor_->receive_timeout = receive_timeout; + r_ctor_->control_task_stack_size_bytes = control_task_stack_size_bytes; r_ctor_->log_level = log_level; return r_ctor_; }), py::arg("server_address") = std::string(), py::arg("rtsp_path") = std::string(), py::arg("receive_timeout") = std::chrono::seconds(5), + py::arg("control_task_stack_size_bytes") = + espp::RtspSession::Config::default_control_task_stack_size_bytes, py::arg("log_level") = espp::Logger::Verbosity::WARN) .def_readwrite("server_address", &espp::RtspSession::Config::server_address, "/< The address of the server") @@ -3384,14 +3421,16 @@ void py_init_module_espp(py::module &m) { "/< The RTSP path of the session") .def_readwrite("receive_timeout", &espp::RtspSession::Config::receive_timeout, "/< The timeout for receiving data. Should be > 0.") + .def_readwrite("control_task_stack_size_bytes", + &espp::RtspSession::Config::control_task_stack_size_bytes, + "/< RTSP control-task stack size, in bytes") .def_readwrite("sdp_generator", &espp::RtspSession::Config::sdp_generator, "") .def_readwrite("log_level", &espp::RtspSession::Config::log_level, "/< The log level of the session"); } // end of inner classes & enums of RtspSession pyClassRtspSession - .def(py::init, const espp::RtspSession::Config &>(), - py::arg("control_socket"), py::arg("config")) + .def(py::init, const espp::RtspSession::Config &>()) .def("get_session_id", &espp::RtspSession::get_session_id, "/ @brief Get the session id\n/ @return The session id") .def("is_closed", &espp::RtspSession::is_closed, @@ -3419,12 +3458,24 @@ void py_init_module_espp(py::module &m) { "/ Send an RTP packet on a specific track\n/ @param track_id The track to send on\n/ " "@param packet The RTP packet to send\n/ @return True if the packet was sent " "successfully, False otherwise") + .def("send_rtp_packet", + py::overload_cast>(&espp::RtspSession::send_rtp_packet), + py::arg("track_id"), py::arg("packet_data"), + "/ Send a serialized RTP packet on a specific track.\n/ @param track_id The track to " + "send on\n/ @param packet_data Serialized RTP packet bytes\n/ @return True if the " + "packet was sent successfully, False otherwise") .def("send_rtp_packet", py::overload_cast(&espp::RtspSession::send_rtp_packet), py::arg("packet"), "/ Send an RTP packet to the client (backward compat — sends on default track 0)\n/ " "@param packet The RTP packet to send\n/ @return True if the packet was sent " "successfully, False otherwise") + .def("send_rtp_packet", + py::overload_cast>(&espp::RtspSession::send_rtp_packet), + py::arg("packet_data"), + "/ Send a serialized RTP packet to the client (default track 0).\n/ @param packet_data " + "Serialized RTP packet bytes\n/ @return True if the packet was sent successfully, False " + "otherwise") .def("send_rtcp_packet", py::overload_cast(&espp::RtspSession::send_rtcp_packet), py::arg("track_id"), py::arg("packet"), diff --git a/lib/python_bindings/rtps_bindings.cpp b/lib/python_bindings/rtps_bindings.cpp new file mode 100644 index 000000000..fd1a84d84 --- /dev/null +++ b/lib/python_bindings/rtps_bindings.cpp @@ -0,0 +1,212 @@ +// Hand-written pybind11 bindings for the `rtps` component (RtpsParticipant). +// +// Why hand-written (like cdr): RtpsParticipant exposes std::function callbacks (some taking +// std::span, which has no pybind caster), std::span publish/on_sample APIs, and a +// large nest of helper structs. litgen/srcmlcpp cannot bind these usefully. This shim exposes a +// clean, GIL-correct Python API: +// - publish(topic, bytes) / ReaderConfig.on_sample = callable(bytes) +// - discovery callbacks delivering ParticipantProxy / EndpointProxy objects +// - participant lifecycle + discovery queries +// +// It is kept out of the generated pybind_espp.cpp so regeneration never clobbers it. + +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +#include "rtps.hpp" + +namespace py = pybind11; +using Rtps = espp::RtpsParticipant; + +namespace { + +py::bytes to_bytes(std::span s) { + return py::bytes(reinterpret_cast(s.data()), s.size()); +} + +std::vector bytes_to_vec(const py::bytes &data) { + std::string s = data; + return std::vector(s.begin(), s.end()); +} + +// Reader config exposed to Python: like Rtps::ReaderConfig but `on_sample` is a Python callable +// taking `bytes` (the raw CDR sample). add_reader() adapts it into the span-based C++ callback. +struct PyReaderConfig { + std::string topic_name{}; + std::string type_name{"std_msgs/msg/UInt32"}; + Rtps::ReliabilityKind reliability{Rtps::ReliabilityKind::BEST_EFFORT}; + std::string multicast_group{}; + uint32_t entity_index{0}; + py::function on_sample{}; +}; + +Rtps::ReaderConfig to_reader_config(const PyReaderConfig &pc) { + Rtps::ReaderConfig rc; + rc.topic_name = pc.topic_name; + rc.type_name = pc.type_name; + rc.reliability = pc.reliability; + rc.multicast_group = pc.multicast_group; + rc.entity_index = pc.entity_index; + if (pc.on_sample) { + py::function cb = pc.on_sample; + rc.on_sample = [cb](std::span data) { + // Runs on an rtps receive thread; take the GIL before touching Python. + py::gil_scoped_acquire gil; + cb(to_bytes(data)); + }; + } + return rc; +} + +} // namespace + +void py_init_rtps(py::module &m) { + auto rtps = py::class_(m, "RtpsParticipant", py::dynamic_attr(), + "Cross-platform RTPS protocol participant (discovery + best-effort " + "CDR-over-RTPS user data)."); + + py::enum_(rtps, "ReliabilityKind") + .value("BEST_EFFORT", Rtps::ReliabilityKind::BEST_EFFORT) + .value("RELIABLE", Rtps::ReliabilityKind::RELIABLE); + + py::class_(rtps, "GuidPrefix") + .def(py::init<>()) + .def_readonly("value", &Rtps::GuidPrefix::value) + .def("to_string", &Rtps::GuidPrefix::to_string) + .def("__repr__", &Rtps::GuidPrefix::to_string); + + py::class_(rtps, "EntityId") + .def(py::init<>()) + .def_readonly("value", &Rtps::EntityId::value) + .def("to_string", &Rtps::EntityId::to_string) + .def("__repr__", &Rtps::EntityId::to_string); + + py::class_(rtps, "Guid") + .def(py::init<>()) + .def_readonly("prefix", &Rtps::Guid::prefix) + .def_readonly("entity_id", &Rtps::Guid::entity_id) + .def("to_string", &Rtps::Guid::to_string) + .def("__repr__", &Rtps::Guid::to_string); + + auto locator = py::class_(rtps, "Locator"); + py::enum_(locator, "Kind") + .value("INVALID", Rtps::Locator::Kind::INVALID) + .value("UDP_V4", Rtps::Locator::Kind::UDP_V4); + locator.def(py::init<>()) + .def_static("udp_v4", &Rtps::Locator::udp_v4, py::arg("ipv4_address"), py::arg("port")) + .def_readwrite("kind", &Rtps::Locator::kind) + .def_readwrite("port", &Rtps::Locator::port) + .def("address_string", &Rtps::Locator::address_string); + + py::class_(rtps, "PortMapping") + .def(py::init<>()) + .def_readwrite("metatraffic_multicast", &Rtps::PortMapping::metatraffic_multicast) + .def_readwrite("metatraffic_unicast", &Rtps::PortMapping::metatraffic_unicast) + .def_readwrite("user_multicast", &Rtps::PortMapping::user_multicast) + .def_readwrite("user_unicast", &Rtps::PortMapping::user_unicast); + + py::class_(rtps, "ParticipantProxy") + .def_readonly("participant_guid", &Rtps::ParticipantProxy::participant_guid) + .def_readonly("guid_prefix", &Rtps::ParticipantProxy::guid_prefix) + .def_readonly("name", &Rtps::ParticipantProxy::name) + .def_readonly("enclave", &Rtps::ParticipantProxy::enclave) + .def_readonly("address", &Rtps::ParticipantProxy::address) + .def_readonly("ports", &Rtps::ParticipantProxy::ports) + .def_readonly("builtin_endpoints", &Rtps::ParticipantProxy::builtin_endpoints); + + py::class_(rtps, "EndpointProxy") + .def_readonly("guid", &Rtps::EndpointProxy::guid) + .def_readonly("participant_guid", &Rtps::EndpointProxy::participant_guid) + .def_readonly("topic_name", &Rtps::EndpointProxy::topic_name) + .def_readonly("type_name", &Rtps::EndpointProxy::type_name) + .def_readonly("reliability", &Rtps::EndpointProxy::reliability) + .def_readonly("is_reader", &Rtps::EndpointProxy::is_reader) + .def_readonly("expects_inline_qos", &Rtps::EndpointProxy::expects_inline_qos) + .def_readonly("unicast_locator", &Rtps::EndpointProxy::unicast_locator) + .def_readonly("multicast_locators", &Rtps::EndpointProxy::multicast_locators); + + py::class_(rtps, "WriterConfig") + .def(py::init<>()) + .def_readwrite("topic_name", &Rtps::WriterConfig::topic_name) + .def_readwrite("type_name", &Rtps::WriterConfig::type_name) + .def_readwrite("reliability", &Rtps::WriterConfig::reliability) + .def_readwrite("multicast_group", &Rtps::WriterConfig::multicast_group) + .def_readwrite("entity_index", &Rtps::WriterConfig::entity_index); + + py::class_(rtps, "ReaderConfig") + .def(py::init<>()) + .def_readwrite("topic_name", &PyReaderConfig::topic_name) + .def_readwrite("type_name", &PyReaderConfig::type_name) + .def_readwrite("reliability", &PyReaderConfig::reliability) + .def_readwrite("multicast_group", &PyReaderConfig::multicast_group) + .def_readwrite("entity_index", &PyReaderConfig::entity_index) + .def_readwrite("on_sample", &PyReaderConfig::on_sample, + "Callable invoked with the raw CDR sample (bytes) on a matching topic."); + + // Config: expose the host-relevant fields. Task configs are left at their defaults. The discovery + // callbacks deliver bound proxy objects (pybind handles the GIL for these std::function casts). + py::class_(rtps, "Config") + .def(py::init<>()) + .def_readwrite("node_name", &Rtps::Config::node_name) + .def_readwrite("domain_id", &Rtps::Config::domain_id) + .def_readwrite("participant_id", &Rtps::Config::participant_id) + .def_readwrite("bind_address", &Rtps::Config::bind_address) + .def_readwrite("advertised_address", &Rtps::Config::advertised_address) + .def_readwrite("metatraffic_multicast_group", &Rtps::Config::metatraffic_multicast_group) + .def_readwrite("user_multicast_group", &Rtps::Config::user_multicast_group) + .def_readwrite("use_multicast_for_user_data", &Rtps::Config::use_multicast_for_user_data) + .def_readwrite("announce_period", &Rtps::Config::announce_period) + .def_readwrite("enclave", &Rtps::Config::enclave) + .def_readwrite("on_participant_discovered", &Rtps::Config::on_participant_discovered) + .def_readwrite("on_endpoint_discovered", &Rtps::Config::on_endpoint_discovered) + .def_readwrite("log_level", &Rtps::Config::log_level) + .def_readwrite("socket_log_level", &Rtps::Config::socket_log_level); + + rtps.def(py::init(), py::arg("config")) + .def("start", &Rtps::start, py::call_guard()) + .def("stop", &Rtps::stop, py::call_guard()) + .def("is_started", &Rtps::is_started) + .def("add_writer", &Rtps::add_writer, py::arg("writer_config")) + .def( + "add_reader", + [](Rtps &self, const PyReaderConfig &rc) { + return self.add_reader(to_reader_config(rc)); + }, + py::arg("reader_config")) + .def("discovered_participants", &Rtps::discovered_participants) + .def("discovered_writers", &Rtps::discovered_writers) + .def("discovered_readers", &Rtps::discovered_readers) + .def("writers", &Rtps::writers) + .def("readers", + [](const Rtps &self) { + // Return the host-friendly reader view (without the C++ span callback). + std::vector out; + for (const auto &r : self.readers()) { + out.push_back({r.topic_name, r.type_name, r.reliability, r.multicast_group, + r.entity_index, py::function{}}); + } + return out; + }) + .def("ports", &Rtps::ports) + .def("participant_guid", &Rtps::participant_guid) + .def("writer_guid", &Rtps::writer_guid, py::arg("index")) + .def("reader_guid", &Rtps::reader_guid, py::arg("index")) + .def( + "publish", + [](Rtps &self, std::string_view topic, const py::bytes &cdr_payload) { + auto vec = bytes_to_vec(cdr_payload); + py::gil_scoped_release release; + return self.publish(topic, std::span{vec.data(), vec.size()}); + }, + py::arg("topic_name"), py::arg("cdr_payload")) + .def_static("compute_port_mapping", &Rtps::compute_port_mapping, py::arg("domain_id"), + py::arg("participant_id")); +} From ebc41ce5bfe080ea2132bd0fc5c00b8acad1a54f Mon Sep 17 00:00:00 2001 From: William Emfinger Date: Thu, 18 Jun 2026 23:16:52 -0500 Subject: [PATCH 28/32] simplify and improve workflow --- lib/README.md | 41 +++++++++++++++-------------- lib/autogenerate_bindings.py | 51 ++++++++++++++++++++++++------------ 2 files changed, 55 insertions(+), 37 deletions(-) diff --git a/lib/README.md b/lib/README.md index 52a00d2cc..10983c918 100644 --- a/lib/README.md +++ b/lib/README.md @@ -92,33 +92,34 @@ pip install -r requirements.txt ```console # start the environment source env/bin/activate -# configure the build once so the qualification fixer has the exact compile flags: -cmake -DCMAKE_EXPORT_COMPILE_COMMANDS=ON -S . -B build python autogenerate_bindings.py ``` -**No manual editing of the generated file is required.** The fixups that used to -be applied by hand are now fully automated: - -- `autogenerate_bindings.py` strips C++20 `requires`-clauses at parse time (so - `Vector2d` and friends parse), then post-processes the generated - `pybind_espp.cpp` to fix the litgen/srcmlcpp bugs: bogus implicit default - constructors (`Logger`, `Task`, the RTSP packetizers, ...), template-class - nested args (`RangeMapper`, `Bezier`, `Vector2d`), - static/instance method duplicates (`Task::get_id`), and `std::shared_ptr` - holders (the RTP packetizer hierarchy, `JpegFrame`). -- It then runs [`fix_generated_bindings.py`](./fix_generated_bindings.py), which - compiles the generated file and applies clang's own - `'Y'; did you mean 'espp::X::Y'?` nested-scope qualifications (the previously - *undocumented* RTSP/rtps edits) plus `def_readwrite` → `def_readonly` for - non-copyable members, iterating until the file compiles cleanly. It needs the - `build/compile_commands.json` from the cmake step above; if it is missing, - generation prints a hint and you can run `python fix_generated_bindings.py` - after configuring the build. +**No manual editing of the generated file is required, and no build/compile step +is needed to fix it.** `autogenerate_bindings.py` strips C++20 `requires`-clauses +at parse time (so `Vector2d` and friends parse), then post-processes the +generated `pybind_espp.cpp` entirely with static string/regex passes +(`_postprocess_generated`) to fix every litgen/srcmlcpp bug that used to be fixed +by hand: + +- bogus implicit default constructors (`Logger`, `Task`, the RTSP packetizers, ...), +- template-class nested args (`RangeMapper`, `Bezier`, `Vector2d`), +- static/instance method duplicates (`Task::get_id`), +- `std::shared_ptr` holders + bases (the RTP packetizer hierarchy, `JpegFrame`), +- unqualified RTSP nested types / nested-struct statics in named-ctor defaults + (`espp::RtspClient::frame_callback_t`, `espp::RtspServer::Config::default_*`, ...), +- `def_readwrite` → `def_readonly` for non-copyable members (`RtspSession::Track` sockets). The litgen dependency is unpinned and recent versions (0.20–0.22) regressed nested-scope/template generation, which is why this automation is needed. +[`fix_generated_bindings.py`](./fix_generated_bindings.py) is kept only as an +optional diagnostic: if a future litgen version introduces *new* unqualified +names, configure the build with +`cmake -DCMAKE_EXPORT_COMPILE_COMMANDS=ON -S . -B build` and run it — it compiles +the generated file and reports clang's `'Y'; did you mean 'espp::X::Y'?` +suggestions so you can extend the static maps in `autogenerate_bindings.py`. + ### Hand-written components (`cdr`, `rtps`) `cdr` and `rtps` are **not** generated by litgen — they are bound by hand in diff --git a/lib/autogenerate_bindings.py b/lib/autogenerate_bindings.py index 65904a47c..5067ccb7f 100644 --- a/lib/autogenerate_bindings.py +++ b/lib/autogenerate_bindings.py @@ -197,11 +197,41 @@ def _fix_class_holders(code: str) -> str: return code +# litgen emits some RTSP nested type names / nested-struct static members unqualified inside the +# named-constructor lambda default arguments (e.g. `frame_callback_t`, `default_accept_task_stack_size_bytes`, +# `RtspSession::Config::...`), which do not resolve at namespace scope. Each bare name below is unique +# to one class in the generated output, so a guarded global replacement is safe. `(? str: + for pattern, replacement in _RTSP_QUALIFICATIONS: + code = re.sub(pattern, replacement, code) + # A def_readwrite on the non-copyable UdpSocket Track members won't compile; make them read-only. + for member in ("rtp_socket", "rtcp_socket"): + code = code.replace( + f'.def_readwrite("{member}", &espp::RtspSession::Track::{member}', + f'.def_readonly("{member}", &espp::RtspSession::Track::{member}', + ) + return code + + def _postprocess_generated(code: str) -> str: code = _fix_implicit_default_ctors(code) code = _fix_template_class_nested(code) code = _remove_static_instance_dups(code) code = _fix_class_holders(code) + code = _fix_rtsp_qualifications(code) return code @@ -412,23 +442,10 @@ def autogenerate() -> None: with open(pydef_file, "w") as f: f.write(code) print(f"Post-processed {pydef_file}") - - # Apply the compiler-driven nested-scope qualification fixes (RTSP/rtps), so the generated file - # compiles with zero manual edits. Requires a configured build (compile_commands.json); if that - # is missing, skip with a hint rather than failing. - try: - import fix_generated_bindings - if os.path.exists(fix_generated_bindings.COMPILE_DB): - print("Applying compiler-driven qualification fixes...") - fix_generated_bindings.main() - else: - print( - "Skipping qualification fixer: no build/compile_commands.json. Configure once with\n" - " cmake -DCMAKE_EXPORT_COMPILE_COMMANDS=ON .. (from lib/build), then run\n" - " python fix_generated_bindings.py" - ) - except Exception as exc: # noqa: BLE001 - generation should not hard-fail on the optional fixer - print(f"Qualification fixer did not run ({exc}); run python fix_generated_bindings.py manually.") + # NOTE: all fixups are applied statically in _postprocess_generated() above, so the generated + # file compiles with no manual edits and without a build step. fix_generated_bindings.py is kept + # as an optional diagnostic: if a future litgen version introduces *new* unqualified names, run + # it to have clang's "did you mean ..." suggestions point them out (and to extend the static maps). if __name__ == "__main__": From 138b5348d2dfa7a8ca7e6bb70f3a2c9f195931bb Mon Sep 17 00:00:00 2001 From: William Emfinger Date: Thu, 18 Jun 2026 23:22:01 -0500 Subject: [PATCH 29/32] Potential fix for pull request finding 'File is not always closed' Co-authored-by: Copilot Autofix powered by AI <223894421+github-code-quality[bot]@users.noreply.github.com> --- lib/fix_generated_bindings.py | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/lib/fix_generated_bindings.py b/lib/fix_generated_bindings.py index 5a5c89254..787a6df01 100644 --- a/lib/fix_generated_bindings.py +++ b/lib/fix_generated_bindings.py @@ -35,7 +35,8 @@ def _compile_command() -> list[str]: f" cmake -DCMAKE_EXPORT_COMPILE_COMMANDS=ON ..\n" f"from lib/build so the exact compile flags are available." ) - db = json.load(open(COMPILE_DB)) + with open(COMPILE_DB) as f: + db = json.load(f) entry = next(c for c in db if c["file"].endswith("pybind_espp.cpp")) argv = shlex.split(entry["command"]) # Drop output (-o x.o) and the input file; add syntax-only + all-errors. From b8778b492c7aa4a29eba353c9ed10b7f771cc2d5 Mon Sep 17 00:00:00 2001 From: William Emfinger Date: Fri, 19 Jun 2026 00:02:26 -0500 Subject: [PATCH 30/32] minor fix to socket multicast on xplat --- components/rtps/RELIABLE_RTPS_PLAN.md | 101 ++++++++++++++++++++++++++ components/socket/src/socket.cpp | 11 ++- 2 files changed, 110 insertions(+), 2 deletions(-) create mode 100644 components/rtps/RELIABLE_RTPS_PLAN.md diff --git a/components/rtps/RELIABLE_RTPS_PLAN.md b/components/rtps/RELIABLE_RTPS_PLAN.md new file mode 100644 index 000000000..1205a05db --- /dev/null +++ b/components/rtps/RELIABLE_RTPS_PLAN.md @@ -0,0 +1,101 @@ +# Plan: Reliable RTPS (HEARTBEAT / ACKNACK) — separate PR + +Status: **planned, not started.** This is a fast-follow to the best-effort +CDR-over-RTPS data path and should land as its own PR after that work is merged +and tested. + +## Goal + +Add interoperable RELIABLE delivery for user data: a stateful writer that +retains history and retransmits, and a stateful reader that detects gaps and +requests retransmission. Best-effort behavior must remain unchanged for +endpoints that advertise `BEST_EFFORT`. + +## Why this is a bigger change + +The current user-data path is **stateless** — `publish()` builds a `DATA` +message and fires it once; the reader dispatches whatever arrives. Reliability +requires **per-endpoint state machines** and a **writer history cache**, plus +new submessages and timers. The reliable handshake only runs between endpoints +that both advertise `RELIABLE` (discovered via SEDP). + +## What Tier 1 already gives us + +- Per-writer monotonic sequence numbers (`next_user_data_sequence_number`). +- Writer-GUID-based receive routing and discovered-endpoint reliability flags. +- Standard `DATA` submessage build/parse and the CDR payload path. +- `parse_data_submessage` already honors endianness and skips inline QoS. + +## Work breakdown + +### 1. Submessage codecs (~1 day) +- `HEARTBEAT` (0x07): `{readerId, writerId, firstSN, lastSN, count}` + flags + (Final `F`, Liveliness `L`). +- `ACKNACK` (0x06): `{readerId, writerId, readerSNState, count}` + Final flag. +- `GAP` (0x08): `{readerId, writerId, gapStart, gapList}` (can defer to a + follow-up; needed for irrelevant/removed samples). +- **`SequenceNumberSet`** encoding/parsing: `{bitmapBase (SN), numBits (u32), + bitmap (ceil(numBits/32) u32 words)}`. This is the fiddliest piece — unit-test + it in isolation (round-trip + boundary cases: empty set, 256-bit max, base + alignment). +- Wire each new submessage through `Message::serialize` / `Message::parse` + (they already carry arbitrary submessages; only the payload codecs are new). + +### 2. Reliable writer (~2–3 days) +- **History cache**: retain `(SN -> serialized DATA)` until acked by all matched + reliable readers; cap by a configurable depth (KEEP_LAST) and drop oldest. +- **Periodic HEARTBEAT** task (reuse the `Task` pattern): announce + `(firstSN, lastSN)` with an incrementing count to each matched reliable + reader's unicast locator. +- **ReaderProxy** per matched reliable reader: highest contiguous acked SN + + requested (nacked) set. +- **On ACKNACK**: advance the acked watermark, resend the nacked SNs still in + history, dedup by `count`. +- Send a final HEARTBEAT (F flag) after a burst to prompt a prompt ACKNACK. + +### 3. Reliable reader (~2–3 days) +- **WriterProxy** per matched reliable writer: highest contiguous received SN + + missing set + last heartbeat count. +- **On HEARTBEAT**: compute missing SNs in `(firstSN, lastSN)`, reply with an + `ACKNACK` carrying the `SequenceNumberSet` of missing SNs (or an ack-all when + caught up); honor the F/L flags and dedup by count. +- **In-order delivery**: buffer out-of-order samples and release to `on_sample` + in SN order; bound the reorder buffer. +- Send ACKNACK to the writer's unicast locator (from discovery). + +### 4. Glue, timers, safety (~1–2 days) +- Run the handshake only when both endpoints advertise `RELIABLE`. +- Heartbeat period / nack-response / retransmit timers via `Task`; jitter to + avoid sync storms. +- Thread-safety: the history cache and proxy maps are touched by the publish + path, the receive path, and timer tasks — guard with a dedicated mutex + (distinct from `mutex_`/`receivers_mutex_`; document lock ordering). +- Remove the "reliable not implemented; sending best-effort" downgrade warnings + in `publish()` and `handle_user_message`. + +### 5. Interop validation (~2–3 days, often dominates) +- Round-trip against another espp participant first. +- Then Fast DDS / ROS 2 with Wireshark: confirm HEARTBEAT/ACKNACK exchange, + `SequenceNumberSet` bitmaps, retransmission, and that dropped packets recover. +- Test packet loss explicitly (drop a percentage in a test transport or via + `tc netem`). + +## Estimate + +- **Minimal happy-path** (periodic heartbeat, retransmit-on-any-nack, + ack-up-to-lastSN, no GAP): ~3–5 days. +- **Robust** (proper bitmap nacks, GAP, counts, reorder buffer, edge cases): + ~1.5–2.5 weeks including interop debugging. + +## Suggested PR sequencing + +1. Submessage codecs + `SequenceNumberSet` with unit tests (item 1) — small, + reviewable, no behavior change. +2. Reliable writer (item 2). +3. Reliable reader (item 3) + glue (item 4) + interop validation (item 5). + +## Out of scope (later) + +- Durability beyond VOLATILE (TRANSIENT_LOCAL history replay to late-joiners). +- Full QoS matching/incompatibility reporting. +- Fragmentation (`DATA_FRAG`) for samples larger than the MTU. diff --git a/components/socket/src/socket.cpp b/components/socket/src/socket.cpp index 87a97c624..6db1f23dd 100644 --- a/components/socket/src/socket.cpp +++ b/components/socket/src/socket.cpp @@ -229,8 +229,15 @@ bool Socket::add_multicast_group(const std::string &multicast_group) { } // Assign the IPv4 multicast source interface, via its IP - // (only necessary if this socket is IPV4 only) - struct in_addr iaddr; + // (only necessary if this socket is IPV4 only). Use INADDR_ANY so the OS picks the default + // multicast interface; leaving iaddr uninitialized passes garbage to IP_MULTICAST_IF, which fails + // with EADDRNOTAVAIL ("Can't assign requested address") on some platforms (e.g. macOS). + struct in_addr iaddr {}; +#if defined(ESP_PLATFORM) + iaddr.s_addr = IPADDR_ANY; +#else + iaddr.s_addr = htonl(INADDR_ANY); +#endif err = setsockopt(socket_, IPPROTO_IP, IP_MULTICAST_IF, reinterpret_cast(&iaddr), sizeof(struct in_addr)); if (err < 0) { From ee8cc9b990a0738ba51ecaf75fc1b8879fc55930 Mon Sep 17 00:00:00 2001 From: William Emfinger Date: Fri, 19 Jun 2026 00:03:01 -0500 Subject: [PATCH 31/32] add pc c++ and python tests for rtps --- lib/python_bindings/rtps_bindings.cpp | 112 +++++++++++++++++++------- pc/README.md | 22 +++++ pc/tests/rtps_common.hpp | 74 +++++++++++++++++ pc/tests/rtps_publisher.cpp | 48 +++++++++++ pc/tests/rtps_pubsub.cpp | 93 +++++++++++++++++++++ pc/tests/rtps_subscriber.cpp | 55 +++++++++++++ python/README.md | 10 +++ python/rtps_publisher.py | 71 ++++++++++++++++ python/rtps_pubsub.py | 109 +++++++++++++++++++++++++ python/rtps_subscriber.py | 76 +++++++++++++++++ 10 files changed, 642 insertions(+), 28 deletions(-) create mode 100644 pc/tests/rtps_common.hpp create mode 100644 pc/tests/rtps_publisher.cpp create mode 100644 pc/tests/rtps_pubsub.cpp create mode 100644 pc/tests/rtps_subscriber.cpp create mode 100644 python/rtps_publisher.py create mode 100644 python/rtps_pubsub.py create mode 100644 python/rtps_subscriber.py diff --git a/lib/python_bindings/rtps_bindings.cpp b/lib/python_bindings/rtps_bindings.cpp index fd1a84d84..be10e5b8a 100644 --- a/lib/python_bindings/rtps_bindings.cpp +++ b/lib/python_bindings/rtps_bindings.cpp @@ -10,14 +10,15 @@ // // It is kept out of the generated pybind_espp.cpp so regeneration never clobbers it. +#include #include #include +#include #include #include #include #include -#include #include #include @@ -37,6 +38,24 @@ std::vector bytes_to_vec(const py::bytes &data) { return std::vector(s.begin(), s.end()); } +// Wrap a Python callable into a C++ std::function that is safe for the rtps component to copy and +// invoke from its background (receive / discovery) threads. The rtps component copies these +// std::functions on threads that do not hold the GIL; capturing the py::function directly would +// inc_ref the Python object without the GIL (a crash). Capturing a shared_ptr instead keeps the +// std::function copies GIL-free, and the callable is invoked / destroyed only under the GIL. +template +std::function wrap_callback(const py::function &fn, + std::function to_py) { + if (!fn) { + return {}; + } + auto cb = std::make_shared(fn); + return [cb, to_py = std::move(to_py)](Arg arg) { + py::gil_scoped_acquire gil; + (*cb)(to_py(arg)); + }; +} + // Reader config exposed to Python: like Rtps::ReaderConfig but `on_sample` is a Python callable // taking `bytes` (the raw CDR sample). add_reader() adapts it into the span-based C++ callback. struct PyReaderConfig { @@ -55,17 +74,53 @@ Rtps::ReaderConfig to_reader_config(const PyReaderConfig &pc) { rc.reliability = pc.reliability; rc.multicast_group = pc.multicast_group; rc.entity_index = pc.entity_index; - if (pc.on_sample) { - py::function cb = pc.on_sample; - rc.on_sample = [cb](std::span data) { - // Runs on an rtps receive thread; take the GIL before touching Python. - py::gil_scoped_acquire gil; - cb(to_bytes(data)); - }; - } + rc.on_sample = wrap_callback>( + pc.on_sample, [](std::span data) -> py::object { return to_bytes(data); }); return rc; } +// Participant config exposed to Python. The discovery callbacks are Python callables (adapted the +// same GIL-safe way as on_sample). Task configs are left at their espp defaults. +struct PyRtpsConfig { + std::string node_name{"espp_rtps"}; + uint16_t domain_id{0}; + uint16_t participant_id{0}; + std::string bind_address{"0.0.0.0"}; + std::string advertised_address{"127.0.0.1"}; + std::string metatraffic_multicast_group{"239.255.0.1"}; + std::string user_multicast_group{"239.255.0.1"}; + bool use_multicast_for_user_data{false}; + std::chrono::milliseconds announce_period{1000}; + std::string enclave{"/"}; + py::function on_participant_discovered{}; + py::function on_endpoint_discovered{}; + espp::Logger::Verbosity log_level{espp::Logger::Verbosity::INFO}; + espp::Logger::Verbosity socket_log_level{espp::Logger::Verbosity::WARN}; +}; + +Rtps::Config to_config(const PyRtpsConfig &pc) { + Rtps::Config c; + c.node_name = pc.node_name; + c.domain_id = pc.domain_id; + c.participant_id = pc.participant_id; + c.bind_address = pc.bind_address; + c.advertised_address = pc.advertised_address; + c.metatraffic_multicast_group = pc.metatraffic_multicast_group; + c.user_multicast_group = pc.user_multicast_group; + c.use_multicast_for_user_data = pc.use_multicast_for_user_data; + c.announce_period = pc.announce_period; + c.enclave = pc.enclave; + c.log_level = pc.log_level; + c.socket_log_level = pc.socket_log_level; + c.on_participant_discovered = wrap_callback( + pc.on_participant_discovered, + [](const Rtps::ParticipantProxy &p) -> py::object { return py::cast(p); }); + c.on_endpoint_discovered = wrap_callback( + pc.on_endpoint_discovered, + [](const Rtps::EndpointProxy &e) -> py::object { return py::cast(e); }); + return c; +} + } // namespace void py_init_rtps(py::module &m) { @@ -151,26 +206,27 @@ void py_init_rtps(py::module &m) { .def_readwrite("on_sample", &PyReaderConfig::on_sample, "Callable invoked with the raw CDR sample (bytes) on a matching topic."); - // Config: expose the host-relevant fields. Task configs are left at their defaults. The discovery - // callbacks deliver bound proxy objects (pybind handles the GIL for these std::function casts). - py::class_(rtps, "Config") + // Config: host-relevant fields. The discovery callbacks are Python callables receiving bound + // proxy objects, adapted GIL-safely (see wrap_callback / PyRtpsConfig). + py::class_(rtps, "Config") .def(py::init<>()) - .def_readwrite("node_name", &Rtps::Config::node_name) - .def_readwrite("domain_id", &Rtps::Config::domain_id) - .def_readwrite("participant_id", &Rtps::Config::participant_id) - .def_readwrite("bind_address", &Rtps::Config::bind_address) - .def_readwrite("advertised_address", &Rtps::Config::advertised_address) - .def_readwrite("metatraffic_multicast_group", &Rtps::Config::metatraffic_multicast_group) - .def_readwrite("user_multicast_group", &Rtps::Config::user_multicast_group) - .def_readwrite("use_multicast_for_user_data", &Rtps::Config::use_multicast_for_user_data) - .def_readwrite("announce_period", &Rtps::Config::announce_period) - .def_readwrite("enclave", &Rtps::Config::enclave) - .def_readwrite("on_participant_discovered", &Rtps::Config::on_participant_discovered) - .def_readwrite("on_endpoint_discovered", &Rtps::Config::on_endpoint_discovered) - .def_readwrite("log_level", &Rtps::Config::log_level) - .def_readwrite("socket_log_level", &Rtps::Config::socket_log_level); - - rtps.def(py::init(), py::arg("config")) + .def_readwrite("node_name", &PyRtpsConfig::node_name) + .def_readwrite("domain_id", &PyRtpsConfig::domain_id) + .def_readwrite("participant_id", &PyRtpsConfig::participant_id) + .def_readwrite("bind_address", &PyRtpsConfig::bind_address) + .def_readwrite("advertised_address", &PyRtpsConfig::advertised_address) + .def_readwrite("metatraffic_multicast_group", &PyRtpsConfig::metatraffic_multicast_group) + .def_readwrite("user_multicast_group", &PyRtpsConfig::user_multicast_group) + .def_readwrite("use_multicast_for_user_data", &PyRtpsConfig::use_multicast_for_user_data) + .def_readwrite("announce_period", &PyRtpsConfig::announce_period) + .def_readwrite("enclave", &PyRtpsConfig::enclave) + .def_readwrite("on_participant_discovered", &PyRtpsConfig::on_participant_discovered) + .def_readwrite("on_endpoint_discovered", &PyRtpsConfig::on_endpoint_discovered) + .def_readwrite("log_level", &PyRtpsConfig::log_level) + .def_readwrite("socket_log_level", &PyRtpsConfig::socket_log_level); + + rtps.def(py::init([](const PyRtpsConfig &pc) { return std::make_unique(to_config(pc)); }), + py::arg("config")) .def("start", &Rtps::start, py::call_guard()) .def("stop", &Rtps::stop, py::call_guard()) .def("is_started", &Rtps::is_started) diff --git a/pc/README.md b/pc/README.md index 3c9326028..5475a8167 100644 --- a/pc/README.md +++ b/pc/README.md @@ -36,3 +36,25 @@ To run a test, you can simply run the executable from the terminal: # if macos / linux: ./build/udp_client ``` + +### RTPS tests + +`rtps_pubsub`, `rtps_publisher`, and `rtps_subscriber` exercise the `rtps` +component end to end (SPDP/SEDP discovery + best-effort CDR-over-RTPS user data). + +- `rtps_pubsub [advertised_ipv4] [run_seconds]` is self-contained: it runs a + publisher and a subscriber in one process and exits 0 if samples were received. +- `rtps_publisher [topic] [advertised_ipv4] [period_ms]` and + `rtps_subscriber [topic] [advertised_ipv4]` are standalone and interoperate + with each other, with the Python `rtps_publisher.py`/`rtps_subscriber.py` (and + `rtps_host.py`) in `../python`, and with embedded ESPP RTPS participants. + +```console +# self-contained smoke test (auto-detects the local IPv4): +./build/rtps_pubsub +# cross-process / cross-language (use a real interface IP on both sides): +./build/rtps_subscriber espp/test/counter 192.168.1.50 # terminal 1 +./build/rtps_publisher espp/test/counter 192.168.1.50 # terminal 2 +``` + +Note: RTPS discovery is multicast, so these require a multicast-capable network. diff --git a/pc/tests/rtps_common.hpp b/pc/tests/rtps_common.hpp new file mode 100644 index 000000000..57583cdb6 --- /dev/null +++ b/pc/tests/rtps_common.hpp @@ -0,0 +1,74 @@ +// Shared helpers for the RTPS host-side tests (pc/tests/rtps_*.cpp). +#pragma once + +#include +#include +#include +#include +#include + +#if defined(_WIN32) +#include +#include +#else +#include +#include +#include +#include +#endif + +#include "espp.hpp" + +namespace rtps_test { + +/// Best-effort guess of this machine's primary outbound IPv4 address. RTPS discovery is multicast, +/// so a real interface address (not 127.0.0.1) is needed for cross-host / cross-process discovery. +inline std::string guess_local_ipv4() { +#if defined(_WIN32) + WSADATA wsa; + WSAStartup(MAKEWORD(2, 2), &wsa); +#endif + std::string ip = "127.0.0.1"; + int sock = static_cast(::socket(AF_INET, SOCK_DGRAM, 0)); + if (sock >= 0) { + sockaddr_in remote{}; + remote.sin_family = AF_INET; + remote.sin_port = htons(53); + ::inet_pton(AF_INET, "8.8.8.8", &remote.sin_addr); + if (::connect(sock, reinterpret_cast(&remote), sizeof(remote)) == 0) { + sockaddr_in local{}; + socklen_t len = sizeof(local); + if (::getsockname(sock, reinterpret_cast(&local), &len) == 0) { + char buf[INET_ADDRSTRLEN] = {0}; + if (::inet_ntop(AF_INET, &local.sin_addr, buf, sizeof(buf))) { + ip = buf; + } + } + } +#if defined(_WIN32) + ::closesocket(sock); +#else + ::close(sock); +#endif + } + return ip; +} + +/// Serialize a uint32 as an encapsulated little-endian CDR payload (matches std_msgs/msg/UInt32). +inline std::vector serialize_uint32(uint32_t value) { + espp::CdrWriter writer; // defaults: CDR_LE with a 4-byte encapsulation header + writer.write(value); + return writer.take_buffer(); +} + +/// Parse a uint32 from an encapsulated CDR payload, or std::nullopt if invalid. +inline std::optional deserialize_uint32(std::span cdr) { + espp::CdrReader reader(cdr); + uint32_t value = 0; + if (!reader.valid() || !reader.read(value)) { + return std::nullopt; + } + return value; +} + +} // namespace rtps_test diff --git a/pc/tests/rtps_publisher.cpp b/pc/tests/rtps_publisher.cpp new file mode 100644 index 000000000..a8ea31b01 --- /dev/null +++ b/pc/tests/rtps_publisher.cpp @@ -0,0 +1,48 @@ +// Standalone RTPS publisher: announces a writer and periodically publishes std_msgs/msg/UInt32 +// samples. Pair it with rtps_subscriber (C++), python/rtps_subscriber.py, or python/rtps_host.py. +// +// Usage: rtps_publisher [topic] [advertised_ipv4] [period_ms] + +#include +#include + +#include "espp.hpp" +#include "rtps_common.hpp" + +using namespace std::chrono_literals; + +int main(int argc, char **argv) { + espp::Logger logger({.tag = "rtps_publisher", .level = espp::Logger::Verbosity::INFO}); + + const std::string topic = argc > 1 ? argv[1] : "espp/test/counter"; + const std::string address = argc > 2 ? argv[2] : rtps_test::guess_local_ipv4(); + const int period_ms = argc > 3 ? std::atoi(argv[3]) : 1000; + + espp::RtpsParticipant participant({ + .node_name = "espp_publisher", + .participant_id = 10, + .advertised_address = address, + .announce_period = 500ms, + .on_endpoint_discovered = + [&logger](const auto &endpoint) { + logger.info("discovered {} '{}'", endpoint.is_reader ? "reader" : "writer", + endpoint.topic_name); + }, + }); + participant.add_writer({.topic_name = topic}); + + if (!participant.start()) { + logger.error("Failed to start participant (is multicast networking available?)"); + return 1; + } + logger.info("publishing on '{}' from {} every {}ms (Ctrl-C to stop)", topic, address, period_ms); + + uint32_t value = 0; + while (true) { + ++value; + bool sent = participant.publish(topic, rtps_test::serialize_uint32(value)); + logger.info("publish {} -> {}", value, sent ? "sent" : "no destinations yet"); + std::this_thread::sleep_for(std::chrono::milliseconds(period_ms)); + } + return 0; +} diff --git a/pc/tests/rtps_pubsub.cpp b/pc/tests/rtps_pubsub.cpp new file mode 100644 index 000000000..c1670246d --- /dev/null +++ b/pc/tests/rtps_pubsub.cpp @@ -0,0 +1,93 @@ +// Self-contained RTPS test: two participants (a publisher and a subscriber) in one process exchange +// std_msgs/msg/UInt32 samples over best-effort CDR-over-RTPS, exercising SPDP/SEDP discovery and +// the user-data path end to end. Exits 0 if the subscriber received samples, 1 otherwise. +// +// Usage: rtps_pubsub [advertised_ipv4] [run_seconds] + +#include +#include +#include + +#include "espp.hpp" +#include "rtps_common.hpp" + +using namespace std::chrono_literals; + +int main(int argc, char **argv) { + espp::Logger logger({.tag = "rtps_pubsub", .level = espp::Logger::Verbosity::INFO}); + + const std::string address = argc > 1 ? argv[1] : rtps_test::guess_local_ipv4(); + const int run_seconds = argc > 2 ? std::atoi(argv[2]) : 8; + const std::string topic = "espp/test/counter"; + logger.info("advertising on {} for {}s, topic '{}'", address, run_seconds, topic); + + std::atomic received_count{0}; + std::atomic last_received{0}; + + // --- Subscriber participant --- + espp::RtpsParticipant subscriber({ + .node_name = "espp_pubsub_subscriber", + .participant_id = 11, + .advertised_address = address, + .announce_period = 200ms, + .log_level = espp::Logger::Verbosity::WARN, + }); + subscriber.add_reader({ + .topic_name = topic, + .on_sample = + [&](std::span cdr) { + if (auto value = rtps_test::deserialize_uint32(cdr)) { + received_count++; + last_received = *value; + } + }, + }); + + // --- Publisher participant --- + espp::RtpsParticipant publisher({ + .node_name = "espp_pubsub_publisher", + .participant_id = 10, + .advertised_address = address, + .announce_period = 200ms, + .log_level = espp::Logger::Verbosity::WARN, + }); + publisher.add_writer({.topic_name = topic}); + + if (!subscriber.start() || !publisher.start()) { + logger.error("Failed to start participants (is multicast networking available?)"); + return 1; + } + + // Give SPDP/SEDP discovery a moment to match the writer and reader. + logger.info("waiting for discovery..."); + for (int i = 0; i < 50; i++) { + if (!publisher.discovered_readers().empty() && !subscriber.discovered_writers().empty()) { + break; + } + std::this_thread::sleep_for(100ms); + } + logger.info("discovered {} remote reader(s), {} remote writer(s)", + publisher.discovered_readers().size(), subscriber.discovered_writers().size()); + + uint32_t sent_count = 0; + const auto deadline = std::chrono::steady_clock::now() + std::chrono::seconds(run_seconds); + while (std::chrono::steady_clock::now() < deadline) { + uint32_t value = ++sent_count; + if (publisher.publish(topic, rtps_test::serialize_uint32(value))) { + logger.info("published {} -> received so far {} (last={})", value, received_count.load(), + last_received.load()); + } + std::this_thread::sleep_for(500ms); + } + + publisher.stop(); + subscriber.stop(); + + logger.info("done: sent {}, received {}, last value {}", sent_count, received_count.load(), + last_received.load()); + if (received_count == 0) { + logger.error("subscriber received no samples"); + return 1; + } + return 0; +} diff --git a/pc/tests/rtps_subscriber.cpp b/pc/tests/rtps_subscriber.cpp new file mode 100644 index 000000000..501dddfec --- /dev/null +++ b/pc/tests/rtps_subscriber.cpp @@ -0,0 +1,55 @@ +// Standalone RTPS subscriber: announces a reader and prints received std_msgs/msg/UInt32 samples. +// Pair it with rtps_publisher (C++), python/rtps_publisher.py, or python/rtps_host.py. +// +// Usage: rtps_subscriber [topic] [advertised_ipv4] + +#include +#include +#include + +#include "espp.hpp" +#include "rtps_common.hpp" + +using namespace std::chrono_literals; + +int main(int argc, char **argv) { + espp::Logger logger({.tag = "rtps_subscriber", .level = espp::Logger::Verbosity::INFO}); + + const std::string topic = argc > 1 ? argv[1] : "espp/test/counter"; + const std::string address = argc > 2 ? argv[2] : rtps_test::guess_local_ipv4(); + + std::atomic count{0}; + + espp::RtpsParticipant participant({ + .node_name = "espp_subscriber", + .participant_id = 12, + .advertised_address = address, + .announce_period = 500ms, + .on_participant_discovered = + [&logger](const auto &proxy) { + logger.info("discovered participant '{}' at {}", proxy.name, proxy.address); + }, + }); + participant.add_reader({ + .topic_name = topic, + .on_sample = + [&](std::span cdr) { + if (auto value = rtps_test::deserialize_uint32(cdr)) { + logger.info("received {} (#{})", *value, ++count); + } + }, + }); + + if (!participant.start()) { + logger.error("Failed to start participant (is multicast networking available?)"); + return 1; + } + logger.info("subscribed to '{}' on {} (Ctrl-C to stop)", topic, address); + + while (true) { + std::this_thread::sleep_for(5s); + logger.info("status: {} samples received, {} known publisher(s)", count.load(), + participant.discovered_writers().size()); + } + return 0; +} diff --git a/python/README.md b/python/README.md index c54b698e6..d2c806ceb 100644 --- a/python/README.md +++ b/python/README.md @@ -54,6 +54,16 @@ This section gives a brief overview of what the scripts in this folder do. 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. +- `rtps_pubsub.py`, `rtps_publisher.py`, `rtps_subscriber.py`: RTPS tests that + use the **espp Python library** (`espp.RtpsParticipant` + `espp.CdrWriter`/ + `CdrReader`) rather than the pure-stdlib harness. `rtps_pubsub.py` is + self-contained (a publisher and subscriber in one process; exits 0 if samples + were received). `rtps_publisher.py` / `rtps_subscriber.py` are standalone and + interoperate with each other, with the C++ `rtps_publisher`/`rtps_subscriber` + in `../pc`, and with `rtps_host.py`. They take an optional + `[topic] [advertised_ipv4]` (a real interface IP is needed for cross-host + discovery). Note: RTPS discovery is multicast, so these require a + multicast-capable network. - `cobs_demo.py`: Demonstration of ESPP COBS functionality with native Python data types. Shows ESPP encoding/decoding, cross-library compatibility with the cobs-python library, and practical usage examples. Includes design differences explanation and validation diff --git a/python/rtps_publisher.py b/python/rtps_publisher.py new file mode 100644 index 000000000..625de3328 --- /dev/null +++ b/python/rtps_publisher.py @@ -0,0 +1,71 @@ +#!/usr/bin/env python3 +"""Standalone RTPS publisher using the espp Python library. + +Announces a writer and periodically publishes std_msgs/msg/UInt32 samples. Pair it with +rtps_subscriber.py, the C++ rtps_subscriber, or rtps_host.py. + +Usage: python rtps_publisher.py [topic] [advertised_ipv4] [period_seconds] +""" + +import datetime +import socket +import sys +import time + +from support_loader import espp + + +def guess_local_ipv4() -> str: + probe = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) + try: + probe.connect(("8.8.8.8", 53)) + return probe.getsockname()[0] + except OSError: + return "127.0.0.1" + finally: + probe.close() + + +def serialize_uint32(value: int) -> bytes: + writer = espp.CdrWriter() + writer.write_uint32(value) + return bytes(writer.take_buffer()) + + +def main() -> int: + R = espp.RtpsParticipant + topic = sys.argv[1] if len(sys.argv) > 1 else "espp/test/counter" + address = sys.argv[2] if len(sys.argv) > 2 else guess_local_ipv4() + period = float(sys.argv[3]) if len(sys.argv) > 3 else 1.0 + + cfg = R.Config() + cfg.node_name = "py_publisher" + cfg.participant_id = 20 + cfg.advertised_address = address + cfg.announce_period = datetime.timedelta(milliseconds=500) + cfg.on_endpoint_discovered = lambda e: print( + f"discovered {'reader' if e.is_reader else 'writer'} '{e.topic_name}'") + participant = R(cfg) + wc = R.WriterConfig() + wc.topic_name = topic + participant.add_writer(wc) + + if not participant.start(): + print("Failed to start participant (is multicast networking available?)") + return 1 + print(f"publishing on '{topic}' from {address} every {period}s (Ctrl-C to stop)") + + value = 0 + try: + while True: + value += 1 + sent = participant.publish(topic, serialize_uint32(value)) + print(f"publish {value} -> {'sent' if sent else 'no destinations yet'}") + time.sleep(period) + except KeyboardInterrupt: + participant.stop() + return 0 + + +if __name__ == "__main__": + sys.exit(main()) diff --git a/python/rtps_pubsub.py b/python/rtps_pubsub.py new file mode 100644 index 000000000..5e6080e49 --- /dev/null +++ b/python/rtps_pubsub.py @@ -0,0 +1,109 @@ +#!/usr/bin/env python3 +"""Self-contained RTPS test using the espp Python library. + +Creates two participants (a publisher and a subscriber) in one process and exchanges +std_msgs/msg/UInt32 samples over best-effort CDR-over-RTPS, exercising SPDP/SEDP discovery and the +user-data path end to end. Exits 0 if the subscriber received samples, 1 otherwise. + +Usage: python rtps_pubsub.py [advertised_ipv4] [run_seconds] +""" + +import datetime +import socket +import sys +import time + +from support_loader import espp + + +def guess_local_ipv4() -> str: + # RTPS discovery is multicast, so a real interface address (not 127.0.0.1) is needed. + probe = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) + try: + probe.connect(("8.8.8.8", 53)) + return probe.getsockname()[0] + except OSError: + return "127.0.0.1" + finally: + probe.close() + + +def serialize_uint32(value: int) -> bytes: + writer = espp.CdrWriter() # default little-endian CDR with encapsulation header + writer.write_uint32(value) + return bytes(writer.take_buffer()) + + +def deserialize_uint32(data: bytes): + return espp.CdrReader(data).read_uint32() # int, or None on failure + + +def main() -> int: + R = espp.RtpsParticipant + address = sys.argv[1] if len(sys.argv) > 1 else guess_local_ipv4() + run_seconds = int(sys.argv[2]) if len(sys.argv) > 2 else 8 + topic = "espp/test/counter" + print(f"advertising on {address} for {run_seconds}s, topic '{topic}'") + + stats = {"received": 0, "last": 0} + + # --- Subscriber --- + sub_cfg = R.Config() + sub_cfg.node_name = "py_pubsub_subscriber" + sub_cfg.participant_id = 21 + sub_cfg.advertised_address = address + sub_cfg.announce_period = datetime.timedelta(milliseconds=200) + sub_cfg.log_level = espp.Logger.Verbosity.warn + subscriber = R(sub_cfg) + + def on_sample(cdr: bytes): + value = deserialize_uint32(cdr) + if value is not None: + stats["received"] += 1 + stats["last"] = value + + rc = R.ReaderConfig() + rc.topic_name = topic + rc.on_sample = on_sample + subscriber.add_reader(rc) + + # --- Publisher --- + pub_cfg = R.Config() + pub_cfg.node_name = "py_pubsub_publisher" + pub_cfg.participant_id = 20 + pub_cfg.advertised_address = address + pub_cfg.announce_period = datetime.timedelta(milliseconds=200) + pub_cfg.log_level = espp.Logger.Verbosity.warn + publisher = R(pub_cfg) + wc = R.WriterConfig() + wc.topic_name = topic + publisher.add_writer(wc) + + if not subscriber.start() or not publisher.start(): + print("Failed to start participants (is multicast networking available?)") + return 1 + + print("waiting for discovery...") + for _ in range(50): + if publisher.discovered_readers() and subscriber.discovered_writers(): + break + time.sleep(0.1) + print(f"discovered {len(publisher.discovered_readers())} remote reader(s), " + f"{len(subscriber.discovered_writers())} remote writer(s)") + + sent = 0 + deadline = time.monotonic() + run_seconds + while time.monotonic() < deadline: + sent += 1 + if publisher.publish(topic, serialize_uint32(sent)): + print(f"published {sent} -> received so far {stats['received']} (last={stats['last']})") + time.sleep(0.5) + + publisher.stop() + subscriber.stop() + print(f"done: sent {sent}, received {stats['received']}, last value {stats['last']}") + return 0 if stats["received"] else 1 + + +if __name__ == "__main__": + sys.exit(main()) diff --git a/python/rtps_subscriber.py b/python/rtps_subscriber.py new file mode 100644 index 000000000..cc7bc639d --- /dev/null +++ b/python/rtps_subscriber.py @@ -0,0 +1,76 @@ +#!/usr/bin/env python3 +"""Standalone RTPS subscriber using the espp Python library. + +Announces a reader and prints received std_msgs/msg/UInt32 samples. Pair it with rtps_publisher.py, +the C++ rtps_publisher, or rtps_host.py. + +Usage: python rtps_subscriber.py [topic] [advertised_ipv4] +""" + +import datetime +import socket +import sys +import time + +from support_loader import espp + + +def guess_local_ipv4() -> str: + probe = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) + try: + probe.connect(("8.8.8.8", 53)) + return probe.getsockname()[0] + except OSError: + return "127.0.0.1" + finally: + probe.close() + + +def deserialize_uint32(data: bytes): + return espp.CdrReader(data).read_uint32() + + +def main() -> int: + R = espp.RtpsParticipant + topic = sys.argv[1] if len(sys.argv) > 1 else "espp/test/counter" + address = sys.argv[2] if len(sys.argv) > 2 else guess_local_ipv4() + + count = {"n": 0} + + cfg = R.Config() + cfg.node_name = "py_subscriber" + cfg.participant_id = 22 + cfg.advertised_address = address + cfg.announce_period = datetime.timedelta(milliseconds=500) + cfg.on_participant_discovered = lambda p: print( + f"discovered participant '{p.name}' at {p.address}") + participant = R(cfg) + + def on_sample(cdr: bytes): + value = deserialize_uint32(cdr) + if value is not None: + count["n"] += 1 + print(f"received {value} (#{count['n']})") + + rc = R.ReaderConfig() + rc.topic_name = topic + rc.on_sample = on_sample + participant.add_reader(rc) + + if not participant.start(): + print("Failed to start participant (is multicast networking available?)") + return 1 + print(f"subscribed to '{topic}' on {address} (Ctrl-C to stop)") + + try: + while True: + time.sleep(5) + print(f"status: {count['n']} samples received, " + f"{len(participant.discovered_writers())} known publisher(s)") + except KeyboardInterrupt: + participant.stop() + return 0 + + +if __name__ == "__main__": + sys.exit(main()) From def3151ac8385dfa91ece85878e03af99b4e456e Mon Sep 17 00:00:00 2001 From: William Emfinger Date: Fri, 19 Jun 2026 08:31:23 -0500 Subject: [PATCH 32/32] Potential fix for pull request finding 'File is not always closed' Co-authored-by: Copilot Autofix powered by AI <223894421+github-code-quality[bot]@users.noreply.github.com> --- lib/fix_generated_bindings.py | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/lib/fix_generated_bindings.py b/lib/fix_generated_bindings.py index 787a6df01..fd4f1b23e 100644 --- a/lib/fix_generated_bindings.py +++ b/lib/fix_generated_bindings.py @@ -90,7 +90,8 @@ def _fix_once() -> tuple[int, int]: """Apply one round of fixes. Returns (remaining_error_count, edits_applied).""" err = _compile_errors() error_count = err.count(": error:") - lines = open(PYDEF).read().split("\n") + with open(PYDEF) as f: + lines = f.read().split("\n") # Learn the bare->qualified map from clang's suggestions (only accept espp:: scope suggestions # that genuinely qualify the same identifier, e.g. Y -> espp::X::Y; skip noise like @@ -131,7 +132,8 @@ def _fix_once() -> tuple[int, int]: applied += 1 if applied: - open(PYDEF, "w").write("\n".join(lines)) + with open(PYDEF, "w") as f: + f.write("\n".join(lines)) return error_count, applied