From 083573f0ceac85b8c661bb13b66caacf23a44ec4 Mon Sep 17 00:00:00 2001 From: William Emfinger Date: Wed, 12 Aug 2026 16:19:53 -0500 Subject: [PATCH 1/4] feat(rtps_embedded): Typed Publisher/Subscriber pub/sub (Phase 5) Co-Authored-By: Claude Opus 4.8 (1M context) --- .../rtps_embedded/example/main/main.cpp | 60 +++---- .../rtps_embedded/include/rtps_pubsub.hpp | 169 ++++++++++++++++++ lib/include/espp.hpp | 1 + pc/tests/rtps_typed_pubsub.cpp | 101 +++++++++++ 4 files changed, 294 insertions(+), 37 deletions(-) create mode 100644 components/rtps_embedded/include/rtps_pubsub.hpp create mode 100644 pc/tests/rtps_typed_pubsub.cpp diff --git a/components/rtps_embedded/example/main/main.cpp b/components/rtps_embedded/example/main/main.cpp index 88ebb155f..5b643685e 100644 --- a/components/rtps_embedded/example/main/main.cpp +++ b/components/rtps_embedded/example/main/main.cpp @@ -3,26 +3,20 @@ #include "esp32-ethernet-kit.hpp" -#include "cdr.hpp" #include "logger.hpp" #include "rtps_participant.hpp" +#include "rtps_pubsub.hpp" #include "timer.hpp" using namespace std::chrono_literals; -// std_msgs/msg/String: the reflection-driven cdr component serializes any -// reflectable struct straight to the DDS wire format (cdr::serialize -// emits the 4-byte encapsulation header + classic-CDR body ROS 2 speaks). +// std_msgs/msg/String as a plain reflectable struct. The typed Publisher / +// Subscriber serialize any such struct to the DDS wire format (ROS 2 / classic +// CDR) with no manual (de)serialization in application code. struct StringMsg { std::string data; }; -// The cdr component works in std::byte; the facade publish/on_sample API uses -// uint8_t spans - bridge the two views (same bytes, different value type). -inline std::span u8_span(const std::vector &bytes) { - return {reinterpret_cast(bytes.data()), bytes.size()}; -} - extern "C" void app_main(void) { espp::Logger logger({.tag = "rtps_example", .level = espp::Logger::Verbosity::INFO}); @@ -69,42 +63,34 @@ extern "C" void app_main(void) { return; } - // Reliable writer: publishes are HEARTBEAT/ACKNACK-acknowledged and - // retransmitted to matched readers. - if (!participant.add_writer({ - .topic = pub_topic, - .type_name = type_name, - .reliability = espp::RtpsParticipant::Reliability::RELIABLE, - })) { - logger.error("Failed to add the writer"); - return; - } - - // Best-effort reader: samples arrive as CDR-encapsulated bytes; decode with - // the reflection-driven cdr::deserialize. - if (!participant.add_reader({ - .topic = sub_topic, - .type_name = type_name, - .on_sample = - [&](std::span cdr_payload) { - if (auto msg = cdr::deserialize(std::as_bytes(cdr_payload)); msg) { - logger.info("rx: {}", msg->data); - } - }, - })) { - logger.error("Failed to add the reader"); + // Typed reliable publisher: publish StringMsg structs directly (HEARTBEAT/ + // ACKNACK-acknowledged, retransmitted to matched readers). No manual CDR. + using Reliability = espp::RtpsParticipant::Reliability; + static espp::Publisher publisher(participant, { + .topic = pub_topic, + .type_name = type_name, + .reliability = Reliability::RELIABLE, + }); + // Typed subscriber: receive StringMsg structs directly. + static espp::Subscriber subscriber( + participant, { + .topic = sub_topic, + .type_name = type_name, + .on_message = [&](const StringMsg &msg) { logger.info("rx: {}", msg.data); }, + }); + if (!publisher.is_valid() || !subscriber.is_valid()) { + logger.error("Failed to create the typed publisher/subscriber"); return; } - // Publish a counter periodically; serialization via the cdr component. + // Publish a counter periodically via the typed publisher. static uint32_t counter = 0; espp::Timer publish_timer({ .name = "rtps_pub", .period = std::chrono::milliseconds(CONFIG_RTPS_EXAMPLE_ANNOUNCE_PERIOD_MS), .callback = [&]() { - auto bytes = cdr::serialize(StringMsg{fmt::format("msg {}", counter++)}); - if (bytes && participant.publish(pub_topic, u8_span(*bytes))) { + if (publisher.publish(StringMsg{fmt::format("msg {}", counter++)})) { logger.info("tx: msg {}", counter - 1); } else { logger.warn("tx dropped (history full)"); diff --git a/components/rtps_embedded/include/rtps_pubsub.hpp b/components/rtps_embedded/include/rtps_pubsub.hpp new file mode 100644 index 000000000..62e0e8f29 --- /dev/null +++ b/components/rtps_embedded/include/rtps_pubsub.hpp @@ -0,0 +1,169 @@ +#pragma once + +#include +#include +#include +#include +#include +#include + +#include "cdr.hpp" +#include "rtps_participant.hpp" + +namespace espp { + +/// @brief A type usable with the typed RTPS pub/sub layer. +/// +/// Any reflectable struct the `cdr` component can serialize and deserialize +/// qualifies - no base class, macros, or member functions required. This mirrors +/// the ROS 2 / DDS message model: a plain data struct whose fields map to CDR. +template +concept RtpsMessage = requires(const T &value, std::span bytes) { + { cdr::serialized_size(value) } -> std::convertible_to; + {cdr::deserialize(bytes)}; +}; + +/// @brief Typed publisher: publish reflectable message structs on a topic. +/// +/// A thin, header-only wrapper over espp::RtpsParticipant that removes the manual +/// CDR (de)serialization + byte-span handling of the untyped API. Serialization +/// uses the reflection-driven `cdr` component in ROS 2 / classic-CDR (XCDR1) wire +/// format, into a reused buffer so steady-state publishing does not allocate. +/// +/// @code +/// struct Imu { float ax, ay, az; }; // any reflectable struct +/// espp::Publisher pub(participant, {.topic = "rt/imu", +/// .type_name = "sensor_msgs::msg::dds_::Imu_", +/// .reliability = Reliability::RELIABLE}); +/// pub.publish(Imu{0.1f, 0.2f, 9.8f}); +/// @endcode +/// +/// \note For ROS 2 interop use ROS 2 naming: topic "rt/" and type +/// "::msg::dds_::_" (e.g. "rt/chatter" + +/// "std_msgs::msg::dds_::String_"). +template class Publisher { +public: + /// Configuration for a typed publisher. + struct Config { + std::string topic; ///< DDS topic name. + std::string type_name; ///< DDS type name (must match the peer for interop). + RtpsParticipant::Reliability reliability{ + RtpsParticipant::Reliability::BEST_EFFORT}; ///< Reliability QoS. + }; + + /// Construct and register a writer on the participant. The participant must + /// already be started and must outlive this publisher. Check is_valid() (or + /// the return of publish()) to detect registration failure. + /// \param participant The started participant to publish through. + /// \param config The publisher configuration. + Publisher(RtpsParticipant &participant, const Config &config) + : participant_(&participant) + , topic_(config.topic) { + valid_ = participant_->add_writer({ + .topic = config.topic, + .type_name = config.type_name, + .reliability = config.reliability, + }); + } + + /// \return True if the writer was registered successfully. + [[nodiscard]] bool is_valid() const { return valid_; } + + /// Publish one sample. Serializes into a reused buffer (no steady-state + /// allocation) and hands the CDR bytes to the participant. + /// \param sample The message to publish. + /// \return True on success; false if invalid, serialization failed, or the + /// writer history was full. + bool publish(const T &sample) { + if (!valid_) { + return false; + } + const std::size_t needed = cdr::serialized_size(sample); + if (buffer_.size() < needed) { + buffer_.resize(needed); + } + const auto written = cdr::serialize_into(sample, buffer_); + if (!written) { + return false; + } + return participant_->publish( + topic_, + std::span(reinterpret_cast(buffer_.data()), *written)); + } + +private: + RtpsParticipant *participant_{nullptr}; + std::string topic_; + std::vector buffer_; ///< reused serialization scratch (grows once) + bool valid_{false}; +}; + +/// @brief Typed subscriber: receive reflectable message structs from a topic. +/// +/// A thin, header-only wrapper over espp::RtpsParticipant that deserializes each +/// CDR sample into a T and delivers it to a typed callback, removing the manual +/// byte-span + cdr::deserialize handling of the untyped API. +/// +/// @code +/// espp::Subscriber sub(participant, {.topic = "rt/imu", +/// .type_name = "sensor_msgs::msg::dds_::Imu_", +/// .on_message = [](const Imu &m) { use(m); }}); +/// @endcode +template class Subscriber { +public: + /// Called for each successfully deserialized sample. + /// \note Runs on an engine worker thread - return quickly, do not block. + using message_callback_t = std::function; + + /// Configuration for a typed subscriber. + struct Config { + std::string topic; ///< DDS topic name. + std::string type_name; ///< DDS type name (must match the peer for interop). + RtpsParticipant::Reliability reliability{ + RtpsParticipant::Reliability::BEST_EFFORT}; ///< Reliability QoS. + message_callback_t on_message{nullptr}; ///< Typed sample callback. + }; + + /// Construct and register a reader on the participant. The participant must + /// already be started and must outlive this subscriber. + /// \param participant The started participant to subscribe through. + /// \param config The subscriber configuration. + Subscriber(RtpsParticipant &participant, const Config &config) + : on_message_(config.on_message) { + valid_ = participant.add_reader({ + .topic = config.topic, + .type_name = config.type_name, + .reliability = config.reliability, + .on_sample = + [this](std::span cdr_payload) { + if (!on_message_) { + return; + } + auto sample = cdr::deserialize(std::span( + reinterpret_cast(cdr_payload.data()), cdr_payload.size())); + if (sample) { + on_message_(*sample); + } + }, + }); + } + + /// \return True if the reader was registered successfully. + [[nodiscard]] bool is_valid() const { return valid_; } + +private: + message_callback_t on_message_; + bool valid_{false}; +}; + +/// @brief Helpers for ROS 2 name mangling, so typed interop is turnkey. +namespace ros2 { +/// Map a ROS 2 topic (e.g. "chatter") to its DDS topic name ("rt/chatter"). +inline std::string topic_name(std::string_view ros_topic) { + std::string out = "rt/"; + out += ros_topic; + return out; +} +} // namespace ros2 + +} // namespace espp diff --git a/lib/include/espp.hpp b/lib/include/espp.hpp index 541f4bf35..f4c6d0de0 100644 --- a/lib/include/espp.hpp +++ b/lib/include/espp.hpp @@ -48,6 +48,7 @@ extern "C" { #include "rtp_packetizer.hpp" #include "rtp_types.hpp" #include "rtps_participant.hpp" +#include "rtps_pubsub.hpp" #include "rtsp_client.hpp" #include "rtsp_server.hpp" #include "serialization.hpp" diff --git a/pc/tests/rtps_typed_pubsub.cpp b/pc/tests/rtps_typed_pubsub.cpp new file mode 100644 index 000000000..a8df65eff --- /dev/null +++ b/pc/tests/rtps_typed_pubsub.cpp @@ -0,0 +1,101 @@ +// Typed pub/sub test (Phase 5 of components/rtps_embedded/REFACTOR_PLAN.md). +// +// Exercises espp::Publisher / espp::Subscriber: two facade participants in +// one process exchange a reflectable message struct end-to-end, with no manual +// CDR (de)serialization in the test — the typed layer does it. Also confirms the +// zero-alloc publish path (reused buffer) round-trips a multi-field struct. +// +// Exits 0 when at least kRequired typed samples arrive within the deadline. + +#include +#include +#include +#include +#include +#include +#include + +#include "rtps_participant.hpp" +#include "rtps_pubsub.hpp" + +using namespace std::chrono_literals; + +// A plain reflectable message struct — no base class, macros, or methods. +struct Telemetry { + uint32_t seq; + float value; + std::string label; +}; + +int main() { + constexpr int kRequired = 5; + constexpr auto kDeadline = 20s; + const char *topic = "rt/telemetry"; + const char *type = "espp::msg::dds_::Telemetry_"; + using R = espp::RtpsParticipant; + + std::mutex m; + std::vector got; + std::atomic count{0}; + + R pub({.log_level = espp::Logger::Verbosity::WARN}); + R sub({.log_level = espp::Logger::Verbosity::WARN}); + if (!pub.start() || !sub.start()) { + std::printf("FAIL: start\n"); + return 1; + } + + espp::Publisher publisher(pub, { + .topic = topic, + .type_name = type, + .reliability = R::Reliability::RELIABLE, + }); + espp::Subscriber subscriber(sub, { + .topic = topic, + .type_name = type, + .reliability = R::Reliability::RELIABLE, + .on_message = + [&](const Telemetry &t) { + std::lock_guard lk(m); + got.push_back(t); + count.fetch_add(1); + }, + }); + if (!publisher.is_valid() || !subscriber.is_valid()) { + std::printf("FAIL: publisher/subscriber registration\n"); + return 1; + } + + // Let SEDP match before publishing. + std::this_thread::sleep_for(2s); + + int sent = 0; + const auto start = std::chrono::steady_clock::now(); + while (count.load() < kRequired && std::chrono::steady_clock::now() - start < kDeadline) { + publisher.publish(Telemetry{static_cast(sent), 1.5f * sent, "sample"}); + sent++; + std::this_thread::sleep_for(100ms); + } + + const int n = count.load(); + std::printf("sent=%d received=%d\n", sent, n); + // Verify the typed round-trip preserved fields (not just the count). + bool fields_ok = false; + { + std::lock_guard lk(m); + for (const auto &t : got) { + if (t.label == "sample" && t.value == 1.5f * t.seq) { + fields_ok = true; + break; + } + } + } + pub.stop(); + sub.stop(); + if (n >= kRequired && fields_ok) { + std::printf("PASS\n"); + return 0; + } + std::printf("FAIL: received %d/%d, fields_ok=%d\n", n, kRequired, fields_ok); + return 1; +} From 833bb2dcc87078153fa61a6b0f3902d8a27f7822 Mon Sep 17 00:00:00 2001 From: William Emfinger Date: Wed, 12 Aug 2026 16:37:29 -0500 Subject: [PATCH 2/4] fix(rtps_embedded): Address PR #709 review + static analysis MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit - rtps_pubsub.hpp: include /// (the concept and classes used them via transitive includes only) — Copilot. - Subscriber: capture a shared_ptr copy of the callback instead of `this`, so destroying the Subscriber while the participant still runs cannot dangle the engine-invoked callback (use-after-free) — Copilot. Documented the lifetime contract (stop the participant before tearing down callback-referenced state). - Publisher::publish: guard the reused serialization buffer with a mutex so concurrent publish() from multiple threads is not a data race — Copilot. - rtps_typed_pubsub.cpp: default-initialize Telemetry members (uninitMemberVarNoCtor) and use std::any_of instead of a raw loop (useStlAlgorithm) — the cppcheck findings failing static_analysis. Verified: cppcheck clean on both files (CI args); typed test PASS (sent=5 received=5 with field verification); golden byte-identical; host lib builds. Co-Authored-By: Claude Opus 4.8 (1M context) --- .../rtps_embedded/include/rtps_pubsub.hpp | 33 +++++++++++++++---- pc/tests/rtps_typed_pubsub.cpp | 14 ++++---- 2 files changed, 32 insertions(+), 15 deletions(-) diff --git a/components/rtps_embedded/include/rtps_pubsub.hpp b/components/rtps_embedded/include/rtps_pubsub.hpp index 62e0e8f29..8916f504c 100644 --- a/components/rtps_embedded/include/rtps_pubsub.hpp +++ b/components/rtps_embedded/include/rtps_pubsub.hpp @@ -1,7 +1,11 @@ #pragma once +#include +#include #include #include +#include +#include #include #include #include @@ -70,7 +74,8 @@ template class Publisher { [[nodiscard]] bool is_valid() const { return valid_; } /// Publish one sample. Serializes into a reused buffer (no steady-state - /// allocation) and hands the CDR bytes to the participant. + /// allocation) and hands the CDR bytes to the participant. Thread-safe: + /// concurrent calls are serialized (the reused buffer is mutex-guarded). /// \param sample The message to publish. /// \return True on success; false if invalid, serialization failed, or the /// writer history was full. @@ -78,6 +83,7 @@ template class Publisher { if (!valid_) { return false; } + std::lock_guard lock(mutex_); const std::size_t needed = cdr::serialized_size(sample); if (buffer_.size() < needed) { buffer_.resize(needed); @@ -94,6 +100,7 @@ template class Publisher { private: RtpsParticipant *participant_{nullptr}; std::string topic_; + std::mutex mutex_; ///< guards buffer_ against concurrent publish() std::vector buffer_; ///< reused serialization scratch (grows once) bool valid_{false}; }; @@ -125,24 +132,36 @@ template class Subscriber { }; /// Construct and register a reader on the participant. The participant must - /// already be started and must outlive this subscriber. + /// already be started. + /// + /// \note The registered reader (and thus this subscriber's callback) lives on + /// the participant until the participant is stopped - there is no + /// per-reader removal. The callback holds a shared copy of the user + /// callback, so destroying this Subscriber object is safe (it will not + /// dangle); however, whatever the user callback itself references must + /// outlive the participant. Stop the participant before tearing down + /// state the callback captures. /// \param participant The started participant to subscribe through. /// \param config The subscriber configuration. Subscriber(RtpsParticipant &participant, const Config &config) - : on_message_(config.on_message) { + : on_message_(std::make_shared(config.on_message)) { + // Capture a shared_ptr copy (not `this`): the engine may invoke this + // callback until the participant is stopped, so it must stay valid even if + // the Subscriber object is destroyed first. + auto callback = on_message_; valid_ = participant.add_reader({ .topic = config.topic, .type_name = config.type_name, .reliability = config.reliability, .on_sample = - [this](std::span cdr_payload) { - if (!on_message_) { + [callback](std::span cdr_payload) { + if (!*callback) { return; } auto sample = cdr::deserialize(std::span( reinterpret_cast(cdr_payload.data()), cdr_payload.size())); if (sample) { - on_message_(*sample); + (*callback)(*sample); } }, }); @@ -152,7 +171,7 @@ template class Subscriber { [[nodiscard]] bool is_valid() const { return valid_; } private: - message_callback_t on_message_; + std::shared_ptr on_message_; bool valid_{false}; }; diff --git a/pc/tests/rtps_typed_pubsub.cpp b/pc/tests/rtps_typed_pubsub.cpp index a8df65eff..9f4073038 100644 --- a/pc/tests/rtps_typed_pubsub.cpp +++ b/pc/tests/rtps_typed_pubsub.cpp @@ -7,6 +7,7 @@ // // Exits 0 when at least kRequired typed samples arrive within the deadline. +#include #include #include #include @@ -22,8 +23,8 @@ using namespace std::chrono_literals; // A plain reflectable message struct — no base class, macros, or methods. struct Telemetry { - uint32_t seq; - float value; + uint32_t seq{}; + float value{}; std::string label; }; @@ -83,12 +84,9 @@ int main() { bool fields_ok = false; { std::lock_guard lk(m); - for (const auto &t : got) { - if (t.label == "sample" && t.value == 1.5f * t.seq) { - fields_ok = true; - break; - } - } + fields_ok = std::any_of(got.begin(), got.end(), [](const Telemetry &t) { + return t.label == "sample" && t.value == 1.5f * static_cast(t.seq); + }); } pub.stop(); sub.stop(); From f75157a366cac8661df0fc4278ec4ebaf25abeea Mon Sep 17 00:00:00 2001 From: William Emfinger Date: Wed, 12 Aug 2026 19:22:21 -0500 Subject: [PATCH 3/4] feat(rtps_embedded): Python typed pub/sub (espp.rtps) + address PR #709 review Co-Authored-By: Claude Opus 4.8 (1M context) --- .gitignore | 1 + .../rtps_embedded/example/main/main.cpp | 19 ++- .../rtps_embedded/include/rtps_pubsub.hpp | 19 ++- lib/python_bindings/espp/__init__.py | 4 + lib/python_bindings/espp/rtps.py | 143 ++++++++++++++++++ python/requirements.txt | 1 + python/rtps_messages.py | 50 ++++++ python/rtps_publisher.py | 44 +++--- python/rtps_subscriber.py | 50 +++--- python/rtps_typed_test.py | 84 ++++++++++ 10 files changed, 352 insertions(+), 63 deletions(-) create mode 100644 lib/python_bindings/espp/rtps.py create mode 100644 python/rtps_messages.py create mode 100644 python/rtps_typed_test.py diff --git a/.gitignore b/.gitignore index b71d71888..fcb575a86 100644 --- a/.gitignore +++ b/.gitignore @@ -55,3 +55,4 @@ managed_components/ # docker interop harness build tree (bind-mounted, built in-container) pc/build-linux/ +python/env/ diff --git a/components/rtps_embedded/example/main/main.cpp b/components/rtps_embedded/example/main/main.cpp index 5b643685e..a72f588dd 100644 --- a/components/rtps_embedded/example/main/main.cpp +++ b/components/rtps_embedded/example/main/main.cpp @@ -52,7 +52,10 @@ extern "C" void app_main(void) { constexpr const char *sub_topic = "pc_to_mcu"; constexpr const char *type_name = "std_msgs::msg::String"; - static espp::RtpsParticipant participant({ + // Automatic locals: they RAII-clean up in reverse order on any early return + // (subscriber/publisher stop referencing the participant before it is + // destroyed), and the trailing while(true) keeps them alive in normal use. + espp::RtpsParticipant participant({ .interface_address = interface_address, .on_publisher_matched = [&]() { logger.info("publisher matched a remote reader"); }, .on_subscriber_matched = [&]() { logger.info("subscriber matched a remote writer"); }, @@ -66,13 +69,13 @@ extern "C" void app_main(void) { // Typed reliable publisher: publish StringMsg structs directly (HEARTBEAT/ // ACKNACK-acknowledged, retransmitted to matched readers). No manual CDR. using Reliability = espp::RtpsParticipant::Reliability; - static espp::Publisher publisher(participant, { - .topic = pub_topic, - .type_name = type_name, - .reliability = Reliability::RELIABLE, - }); + espp::Publisher publisher(participant, { + .topic = pub_topic, + .type_name = type_name, + .reliability = Reliability::RELIABLE, + }); // Typed subscriber: receive StringMsg structs directly. - static espp::Subscriber subscriber( + espp::Subscriber subscriber( participant, { .topic = sub_topic, .type_name = type_name, @@ -84,7 +87,7 @@ extern "C" void app_main(void) { } // Publish a counter periodically via the typed publisher. - static uint32_t counter = 0; + uint32_t counter = 0; espp::Timer publish_timer({ .name = "rtps_pub", .period = std::chrono::milliseconds(CONFIG_RTPS_EXAMPLE_ANNOUNCE_PERIOD_MS), diff --git a/components/rtps_embedded/include/rtps_pubsub.hpp b/components/rtps_embedded/include/rtps_pubsub.hpp index 8916f504c..93be7ec4f 100644 --- a/components/rtps_embedded/include/rtps_pubsub.hpp +++ b/components/rtps_embedded/include/rtps_pubsub.hpp @@ -88,20 +88,22 @@ template class Publisher { if (buffer_.size() < needed) { buffer_.resize(needed); } - const auto written = cdr::serialize_into(sample, buffer_); + // Serialize into a byte view of the uint8_t buffer via std::as_writable_bytes + // (the standard, well-defined span conversion - no reinterpret_cast aliasing + // concern). The participant API takes the uint8_t span directly. + const auto written = + cdr::serialize_into(sample, std::as_writable_bytes(std::span(buffer_))); if (!written) { return false; } - return participant_->publish( - topic_, - std::span(reinterpret_cast(buffer_.data()), *written)); + return participant_->publish(topic_, std::span(buffer_.data(), *written)); } private: RtpsParticipant *participant_{nullptr}; std::string topic_; - std::mutex mutex_; ///< guards buffer_ against concurrent publish() - std::vector buffer_; ///< reused serialization scratch (grows once) + std::mutex mutex_; ///< guards buffer_ against concurrent publish() + std::vector buffer_; ///< reused serialization scratch (grows once) bool valid_{false}; }; @@ -158,8 +160,9 @@ template class Subscriber { if (!*callback) { return; } - auto sample = cdr::deserialize(std::span( - reinterpret_cast(cdr_payload.data()), cdr_payload.size())); + // std::as_bytes: the standard, well-defined span -> + // span conversion (no reinterpret_cast aliasing). + auto sample = cdr::deserialize(std::as_bytes(cdr_payload)); if (sample) { (*callback)(*sample); } diff --git a/lib/python_bindings/espp/__init__.py b/lib/python_bindings/espp/__init__.py index 070ae79b5..db5ad3445 100644 --- a/lib/python_bindings/espp/__init__.py +++ b/lib/python_bindings/espp/__init__.py @@ -7,3 +7,7 @@ from ._espp import * # type: ignore # noqa: F403 from ._espp import __version__ # noqa: F401 + +# Pure-Python typed pub/sub layer over the bound RtpsParticipant (accessible as +# ``espp.rtps.Publisher`` / ``espp.rtps.Subscriber``). +from . import rtps # noqa: F401,E402 diff --git a/lib/python_bindings/espp/rtps.py b/lib/python_bindings/espp/rtps.py new file mode 100644 index 000000000..9a30e38d3 --- /dev/null +++ b/lib/python_bindings/espp/rtps.py @@ -0,0 +1,143 @@ +"""Lightweight typed pub/sub over :class:`espp.RtpsParticipant`. + +``espp.RtpsParticipant`` transports CDR-encapsulated bytes. This module pairs it +with a runtime CDR codec so you publish and receive *message objects* instead of +raw bytes -- the Python counterpart to the C++ ``espp::Publisher`` / +``espp::Subscriber`` typed layer (templates can't be bound generically, so the +Python equivalent is this small pure-Python wrapper). + +Any message type works as long as it supplies a CDR codec: + +* an instance ``.serialize() -> bytes`` and a classmethod ``.deserialize(bytes)`` + (this is exactly what `pycdr2 `_ dataclasses + provide, and pycdr2 emits ROS 2 / DDS-compatible CDR, so espp interoperates + with FastDDS and ROS 2 out of the box), **or** +* explicit ``serialize`` / ``deserialize`` callables you pass in, for any other + codec. + +Example (pycdr2):: + + from pycdr2 import make_idl_struct + from pycdr2.types import float64 + import espp + + Vector3 = make_idl_struct("Vector3", "geometry_msgs/msg/Vector3", + {"x": float64, "y": float64, "z": float64}) + + participant = espp.RtpsParticipant(espp.RtpsParticipant.Config()) + participant.start() + pub = espp.rtps.Publisher(participant, "rt/vec", Vector3, reliable=True) + pub.publish(Vector3(x=1.0, y=2.0, z=3.0)) + sub = espp.rtps.Subscriber(participant, "rt/vec", Vector3, + on_message=lambda v: print(v), reliable=True) +""" + +from __future__ import annotations + +from typing import Any, Callable, Optional + + +def ros2_dds_type_name(name: str) -> str: + """Map a ROS 2 type name to its DDS wire type name. + + ``"std_msgs/msg/String"`` -> ``"std_msgs::msg::dds_::String_"``. A name that + already looks like a DDS type name (contains ``"::"``) is returned unchanged. + """ + if "::" in name: + return name + parts = [p for p in name.split("/") if p] + if len(parts) < 2: + return name + *namespace, type_name = parts + return "::".join(namespace) + "::dds_::" + type_name + "_" + + +def _resolve_type_name(message_type: Any, type_name: Optional[str]) -> str: + if type_name is not None: + return type_name + # pycdr2 stores the ROS 2 typename on the generated class. + idl = getattr(message_type, "__idl_typename__", None) + if idl: + return ros2_dds_type_name(idl) + raise ValueError( + "type_name is required: the message type has no __idl_typename__ to derive it from" + ) + + +class Publisher: + """Publish message objects on a topic, serializing them to CDR bytes. + + :param participant: a started :class:`espp.RtpsParticipant`. + :param topic: DDS topic name. + :param message_type: message class supplying ``.serialize()`` (and, for a + pycdr2 type, ``__idl_typename__`` so ``type_name`` can be derived). + :param type_name: DDS type name; derived from ``message_type`` if omitted. + :param reliable: use RELIABLE QoS (default best-effort). + :param serialize: optional ``callable(msg) -> bytes`` overriding + ``message_type.serialize``. + """ + + def __init__( + self, + participant: Any, + topic: str, + message_type: Any = None, + *, + type_name: Optional[str] = None, + reliable: bool = False, + serialize: Optional[Callable[[Any], bytes]] = None, + ) -> None: + self._participant = participant + self._topic = topic + self._serialize = serialize if serialize is not None else (lambda m: m.serialize()) + self.valid = participant.add_writer( + topic=topic, type_name=_resolve_type_name(message_type, type_name), reliable=reliable + ) + + def publish(self, message: Any) -> bool: + """Serialize ``message`` and publish it. Returns True on success.""" + return self._participant.publish(self._topic, self._serialize(message)) + + +class Subscriber: + """Receive message objects from a topic, deserializing CDR bytes for you. + + :param participant: a started :class:`espp.RtpsParticipant`. + :param topic: DDS topic name. + :param message_type: message class supplying ``.deserialize(bytes)`` (and, + for a pycdr2 type, ``__idl_typename__``). + :param on_message: ``callable(msg)`` invoked for each received sample. Runs + on an engine worker thread -- return quickly, do not block. + :param type_name: DDS type name; derived from ``message_type`` if omitted. + :param reliable: use RELIABLE QoS (default best-effort). + :param deserialize: optional ``callable(bytes) -> msg`` overriding + ``message_type.deserialize``. + """ + + def __init__( + self, + participant: Any, + topic: str, + message_type: Any = None, + *, + on_message: Callable[[Any], None], + type_name: Optional[str] = None, + reliable: bool = False, + deserialize: Optional[Callable[[bytes], Any]] = None, + ) -> None: + deser = deserialize if deserialize is not None else (lambda data: message_type.deserialize(data)) + + def _on_sample(data: bytes) -> None: + try: + message = deser(data) + except Exception: + # A malformed / wrong-type sample must not kill the reader thread. + return + on_message(message) + + self.valid = participant.add_reader( + topic=topic, + type_name=_resolve_type_name(message_type, type_name), + reliable=reliable, + on_sample=_on_sample, + ) diff --git a/python/requirements.txt b/python/requirements.txt index ada5129fe..576fcd44d 100644 --- a/python/requirements.txt +++ b/python/requirements.txt @@ -5,3 +5,4 @@ simplejpeg cobs sounddevice matplotlib +pycdr2 diff --git a/python/rtps_messages.py b/python/rtps_messages.py new file mode 100644 index 000000000..2587f587e --- /dev/null +++ b/python/rtps_messages.py @@ -0,0 +1,50 @@ +#!/usr/bin/env python3 +"""Shared pycdr2 message definitions for the RTPS python examples. + +pycdr2 turns a message schema into a CDR codec (``.serialize()`` / +``.deserialize()``) that speaks the same ROS 2 / classic-CDR wire format as the +espp C++ side, so these types interoperate with FastDDS and ROS 2. + +Two ways to define a message with pycdr2: + +* On Python <= 3.12, the idiomatic dataclass form works:: + + from dataclasses import dataclass + from pycdr2 import IdlStruct + from pycdr2.types import float64 + + @dataclass + class Vector3(IdlStruct, typename="geometry_msgs/msg/Vector3"): + x: float64 = 0.0 + y: float64 = 0.0 + z: float64 = 0.0 + +* The functional ``make_idl_struct`` form below works on every Python (including + 3.14, where the dataclass form currently trips over CPython's dataclass + changes). It is otherwise equivalent. +""" + +from pycdr2 import make_idl_struct +from pycdr2.types import array, float64, sequence, uint32, uint64 + +# geometry_msgs/msg/Vector3 — a nested sub-message. +Vector3 = make_idl_struct( + "Vector3", + "geometry_msgs/msg/Vector3", + {"x": float64, "y": float64, "z": float64}, +) + +# A composite message showing the pieces you actually hit in real message types: +# a string, scalars, a NESTED struct, a FIXED array, and a variable SEQUENCE. +Imu = make_idl_struct( + "Imu", + "sensor_msgs/msg/Imu", + { + "frame_id": str, # string + "stamp_ns": uint64, # scalar + "seq": uint32, # scalar + "angular_velocity": Vector3, # nested struct + "orientation_covariance": array[float64, 9], # fixed-size array + "samples": sequence[float64], # variable-length sequence + }, +) diff --git a/python/rtps_publisher.py b/python/rtps_publisher.py index 2ec39ff7a..9e54b8d99 100644 --- a/python/rtps_publisher.py +++ b/python/rtps_publisher.py @@ -1,47 +1,55 @@ #!/usr/bin/env python3 """Standalone RTPS publisher using the espp Python library (embeddedRTPS engine). -Publishes CDR string samples on a topic; defaults follow the ROS 2 conventions for -std_msgs/String on /chatter, so `ros2 topic echo /chatter std_msgs/msg/String` -(with rmw_fastrtps) receives them. Pair it with rtps_subscriber.py or any DDS peer. +Publishes a complex nested message (sensor_msgs/msg/Imu-style: string + scalars + +nested struct + fixed array + variable sequence) using pycdr2 for CDR +(de)serialization and the light espp.rtps.Publisher wrapper. Pair it with +rtps_subscriber.py, a FastDDS peer, or a ROS 2 node (rmw_fastrtps). -Usage: python rtps_publisher.py [topic] [type] [interface_ipv4] [period_seconds] +Requires pycdr2 (see python/requirements.txt). + +Usage: python rtps_publisher.py [topic] [interface_ipv4] [period_seconds] """ -import struct import sys import time import espp - -def serialize_string(text: str) -> bytes: - # Little-endian classic CDR (XCDR1) with a 4-byte encapsulation header - the wire - # format of std_msgs/msg/String: uint32 length (incl. null) + bytes + null. - data = text.encode() + b"\x00" - return b"\x00\x01\x00\x00" + struct.pack(" int: - topic = sys.argv[1] if len(sys.argv) > 1 else "rt/chatter" - type_name = sys.argv[2] if len(sys.argv) > 2 else "std_msgs::msg::dds_::String_" - interface = sys.argv[3] if len(sys.argv) > 3 else "" # "" -> auto-detect - period = float(sys.argv[4]) if len(sys.argv) > 4 else 0.5 + topic = sys.argv[1] if len(sys.argv) > 1 else "rt/imu" + interface = sys.argv[2] if len(sys.argv) > 2 else "" # "" -> auto-detect + period = float(sys.argv[3]) if len(sys.argv) > 3 else 0.5 R = espp.RtpsParticipant participant = R(R.Config(interface_address=interface, log_level=espp.Logger.Verbosity.info)) if not participant.start(): print("failed to start participant") return 1 - if not participant.add_writer(topic=topic, type_name=type_name, reliable=True): + + # Publisher: the DDS type name is derived from the pycdr2 typename + # ("sensor_msgs/msg/Imu" -> "sensor_msgs::msg::dds_::Imu_"). + pub = espp.rtps.Publisher(participant, topic, Imu, reliable=True) + if not pub.valid: print("failed to add writer") return 1 - print(f"publishing '{topic}' ({type_name}) every {period}s; ctrl-c to stop") + print(f"publishing '{topic}' (sensor_msgs/msg/Imu) every {period}s; ctrl-c to stop") count = 0 try: while True: - if participant.publish(topic, serialize_string(f"espp python {count}")): + msg = Imu( + frame_id="imu_link", + stamp_ns=time.time_ns(), + seq=count, + angular_velocity=Vector3(x=0.01 * count, y=0.0, z=-0.01 * count), + orientation_covariance=[0.0] * 9, + samples=[float(count), float(count) + 0.5], + ) + if pub.publish(msg): # pycdr2 serializes msg -> CDR bytes for you count += 1 print(f"sent {count}") time.sleep(period) diff --git a/python/rtps_subscriber.py b/python/rtps_subscriber.py index 355e41f3e..121753030 100644 --- a/python/rtps_subscriber.py +++ b/python/rtps_subscriber.py @@ -1,56 +1,48 @@ #!/usr/bin/env python3 """Standalone RTPS subscriber using the espp Python library (embeddedRTPS engine). -Subscribes to CDR string samples; defaults follow the ROS 2 conventions for -std_msgs/String on /chatter, so `ros2 topic pub /chatter std_msgs/msg/String ...` -(with rmw_fastrtps) is received. Pair it with rtps_publisher.py or any DDS peer. +Subscribes to the complex nested message published by rtps_publisher.py (or any +DDS/ROS 2 peer publishing sensor_msgs/msg/Imu), using pycdr2 for CDR +deserialization and the light espp.rtps.Subscriber wrapper -- the callback +receives fully-decoded message objects, not raw bytes. -Usage: python rtps_subscriber.py [topic] [type] [interface_ipv4] [run_seconds] +Requires pycdr2 (see python/requirements.txt). + +Usage: python rtps_subscriber.py [topic] [interface_ipv4] [run_seconds] """ -import struct import sys import time import espp - -def deserialize_string(data: bytes): - # 4-byte encapsulation header + uint32 length + bytes (incl. trailing null). - if len(data) < 8: - return None - little_endian = data[1] & 0x01 - (length,) = struct.unpack("I", data[4:8]) - if length < 1 or 8 + length > len(data): - return None - return data[8 : 8 + length - 1].decode(errors="replace") +from rtps_messages import Imu def main() -> int: - topic = sys.argv[1] if len(sys.argv) > 1 else "rt/chatter" - type_name = sys.argv[2] if len(sys.argv) > 2 else "std_msgs::msg::dds_::String_" - interface = sys.argv[3] if len(sys.argv) > 3 else "" # "" -> auto-detect - run_seconds = float(sys.argv[4]) if len(sys.argv) > 4 else 30.0 + topic = sys.argv[1] if len(sys.argv) > 1 else "rt/imu" + interface = sys.argv[2] if len(sys.argv) > 2 else "" # "" -> auto-detect + run_seconds = float(sys.argv[3]) if len(sys.argv) > 3 else 30.0 stats = {"received": 0} - def on_sample(data: bytes) -> None: - text = deserialize_string(data) - if text is not None: - stats["received"] += 1 - print(f"received {stats['received']}: {text!r}") + def on_message(msg) -> None: # msg is a fully-decoded Imu + stats["received"] += 1 + print( + f"received {stats['received']}: seq={msg.seq} frame={msg.frame_id!r} " + f"wz={msg.angular_velocity.z:.3f} samples={list(msg.samples)}" + ) R = espp.RtpsParticipant participant = R(R.Config(interface_address=interface, log_level=espp.Logger.Verbosity.info)) if not participant.start(): print("failed to start participant") return 1 - if not participant.add_reader( - topic=topic, type_name=type_name, reliable=True, on_sample=on_sample - ): + sub = espp.rtps.Subscriber(participant, topic, Imu, on_message=on_message, reliable=True) + if not sub.valid: print("failed to add reader") return 1 - print(f"subscribed to '{topic}' ({type_name}) for {run_seconds}s") + print(f"subscribed to '{topic}' (sensor_msgs/msg/Imu) for {run_seconds}s") try: time.sleep(run_seconds) @@ -58,7 +50,7 @@ def on_sample(data: bytes) -> None: pass participant.stop() print(f"done; received={stats['received']}") - return stats["received"] == 0 + return 0 if stats["received"] else 1 if __name__ == "__main__": diff --git a/python/rtps_typed_test.py b/python/rtps_typed_test.py new file mode 100644 index 000000000..7f0e2f40d --- /dev/null +++ b/python/rtps_typed_test.py @@ -0,0 +1,84 @@ +#!/usr/bin/env python3 +"""End-to-end test of the espp.rtps typed pub/sub wrapper with pycdr2. + +Two participants in one process exchange a complex nested message (string + +scalars + nested struct + fixed array + variable sequence) using +espp.rtps.Publisher / Subscriber. Verifies the fields round-trip through real +SEDP/DATA over the espp transport (not just a count). Exits 0 on success. + +Requires pycdr2 (see python/requirements.txt). +""" + +import sys +import threading +import time + +import espp + +from rtps_messages import Imu, Vector3 + +REQUIRED = 5 +TOPIC = "rt/imu_test" + + +def main() -> int: + received = [] + lock = threading.Lock() + + def on_message(msg) -> None: + with lock: + received.append(msg) + + R = espp.RtpsParticipant + pub_p = R(R.Config(log_level=espp.Logger.Verbosity.warn)) + sub_p = R(R.Config(log_level=espp.Logger.Verbosity.warn)) + if not pub_p.start() or not sub_p.start(): + print("FAIL: start") + return 1 + + pub = espp.rtps.Publisher(pub_p, TOPIC, Imu, reliable=True) + sub = espp.rtps.Subscriber(sub_p, TOPIC, Imu, on_message=on_message, reliable=True) + if not pub.valid or not sub.valid: + print("FAIL: registration") + return 1 + + time.sleep(2.0) # let SEDP match + + sent = 0 + deadline = time.monotonic() + 20.0 + while len(received) < REQUIRED and time.monotonic() < deadline: + pub.publish( + Imu( + frame_id="imu_link", + stamp_ns=1000 + sent, + seq=sent, + angular_velocity=Vector3(x=float(sent), y=0.0, z=-float(sent)), + orientation_covariance=[0.0] * 9, + samples=[float(sent), float(sent) + 0.5], + ) + ) + sent += 1 + time.sleep(0.1) + + with lock: + n = len(received) + # Field-level check: the typed round-trip preserved nested + string fields. + fields_ok = any( + m.frame_id == "imu_link" + and m.angular_velocity.z == -float(m.seq) + and list(m.samples) == [float(m.seq), float(m.seq) + 0.5] + for m in received + ) + pub_p.stop() + sub_p.stop() + + print(f"sent={sent} received={n} fields_ok={fields_ok}") + if n >= REQUIRED and fields_ok: + print("PASS") + return 0 + print("FAIL") + return 1 + + +if __name__ == "__main__": + sys.exit(main()) From 68fbf9bbdbe14573bf30d99027def416b0131f97 Mon Sep 17 00:00:00 2001 From: William Emfinger Date: Wed, 12 Aug 2026 20:06:50 -0500 Subject: [PATCH 4/4] fix(rtps_embedded): Reject oversized payloads; give demo msg its own type name Co-Authored-By: Claude Opus 4.8 (1M context) --- .../include/rtps_participant.hpp | 14 +++++++++-- .../rtps_embedded/include/rtps_pubsub.hpp | 12 +++++++--- .../rtps_embedded/src/rtps_participant.cpp | 11 +++++++++ python/rtps_messages.py | 24 ++++++++++++------- python/rtps_publisher.py | 14 +++++------ python/rtps_subscriber.py | 10 ++++---- python/rtps_typed_test.py | 8 +++---- 7 files changed, 64 insertions(+), 29 deletions(-) diff --git a/components/rtps_embedded/include/rtps_participant.hpp b/components/rtps_embedded/include/rtps_participant.hpp index eb2750455..bd47573cd 100644 --- a/components/rtps_embedded/include/rtps_participant.hpp +++ b/components/rtps_embedded/include/rtps_participant.hpp @@ -130,12 +130,22 @@ class RtpsParticipant : public BaseComponent { /// reader pool is exhausted). bool add_reader(const ReaderConfig &config); + /// Maximum size of a single published CDR payload, in bytes. Bounded by the + /// RTPS wire format: a DATA submessage's length (octetsToNextHeader) and the + /// engine's DataSize_t are both 16-bit, so one unfragmented sample cannot + /// exceed 65535 bytes. Larger payloads (e.g. images) require DATA_FRAG + /// fragmentation, which this engine does not yet implement - publish() rejects + /// oversized samples rather than silently truncating them. + static constexpr std::size_t max_payload_size = 65535; + /// Publish a CDR-encapsulated sample on a topic previously registered with /// add_writer(). /// \param topic The topic name used in add_writer(). /// \param cdr_payload The CDR-encapsulated sample (4-byte encapsulation - /// header + CDR body); copied into the writer's history. - /// \return True if the sample was accepted into the writer history. + /// header + CDR body); copied into the writer's history. Must not + /// exceed max_payload_size bytes. + /// \return True if the sample was accepted into the writer history; false if + /// the payload exceeds max_payload_size (see that constant). bool publish(std::string_view topic, std::span cdr_payload); protected: diff --git a/components/rtps_embedded/include/rtps_pubsub.hpp b/components/rtps_embedded/include/rtps_pubsub.hpp index 93be7ec4f..6d75da1c9 100644 --- a/components/rtps_embedded/include/rtps_pubsub.hpp +++ b/components/rtps_embedded/include/rtps_pubsub.hpp @@ -77,14 +77,20 @@ template class Publisher { /// allocation) and hands the CDR bytes to the participant. Thread-safe: /// concurrent calls are serialized (the reused buffer is mutex-guarded). /// \param sample The message to publish. - /// \return True on success; false if invalid, serialization failed, or the - /// writer history was full. + /// \return True on success; false if invalid, serialization failed, the + /// writer history was full, or the serialized size exceeds + /// RtpsParticipant::max_payload_size. bool publish(const T &sample) { if (!valid_) { return false; } - std::lock_guard lock(mutex_); const std::size_t needed = cdr::serialized_size(sample); + if (needed > RtpsParticipant::max_payload_size) { + // Reject before serializing: a sample above the RTPS 16-bit payload limit + // cannot be sent unfragmented (see RtpsParticipant::max_payload_size). + return false; + } + std::lock_guard lock(mutex_); if (buffer_.size() < needed) { buffer_.resize(needed); } diff --git a/components/rtps_embedded/src/rtps_participant.cpp b/components/rtps_embedded/src/rtps_participant.cpp index 7047ccc9f..930fedf2c 100644 --- a/components/rtps_embedded/src/rtps_participant.cpp +++ b/components/rtps_embedded/src/rtps_participant.cpp @@ -1,6 +1,7 @@ #include "rtps_participant.hpp" #include +#include #include "rtps/entities/Domain.hpp" @@ -245,6 +246,16 @@ bool RtpsParticipant::publish(std::string_view topic, std::span c logger_.error("No writer for topic '{}'", topic); return false; } + // Reject rather than silently truncate: DataSize_t (and the RTPS submessage + // length field) is 16-bit, so casting a larger size would wrap. Keep the + // header constant in sync with the engine's actual DataSize_t bound. + static_assert(RtpsParticipant::max_payload_size == std::numeric_limits::max()); + if (cdr_payload.size() > max_payload_size) { + logger_.error("Payload for topic '{}' is {} bytes, exceeds the {}-byte RTPS limit; dropped " + "(large payloads need DATA_FRAG fragmentation, not yet supported)", + topic, cdr_payload.size(), max_payload_size); + return false; + } const auto *change = it->second->newChange(rtps::ChangeKind_t::ALIVE, cdr_payload.data(), static_cast(cdr_payload.size())); if (change == nullptr) { diff --git a/python/rtps_messages.py b/python/rtps_messages.py index 2587f587e..4f781072f 100644 --- a/python/rtps_messages.py +++ b/python/rtps_messages.py @@ -5,6 +5,12 @@ ``.deserialize()``) that speaks the same ROS 2 / classic-CDR wire format as the espp C++ side, so these types interoperate with FastDDS and ROS 2. +NOTE: these are *demo* schemas under demo type names (``espp_examples/msg/...``). +Do not reuse an official ROS 2 type name (e.g. ``sensor_msgs/msg/Imu``) unless the +field layout matches it exactly - DDS peers match on the type name and would then +misdecode a mismatched layout. To talk to a real ROS 2 node, define the message +with that node's exact schema and type name. + Two ways to define a message with pycdr2: * On Python <= 3.12, the idiomatic dataclass form works:: @@ -14,7 +20,7 @@ from pycdr2.types import float64 @dataclass - class Vector3(IdlStruct, typename="geometry_msgs/msg/Vector3"): + class Vector3(IdlStruct, typename="espp_examples/msg/Vector3"): x: float64 = 0.0 y: float64 = 0.0 z: float64 = 0.0 @@ -27,18 +33,20 @@ class Vector3(IdlStruct, typename="geometry_msgs/msg/Vector3"): from pycdr2 import make_idl_struct from pycdr2.types import array, float64, sequence, uint32, uint64 -# geometry_msgs/msg/Vector3 — a nested sub-message. +# A small nested sub-message (demo type name, not an official ROS 2 type). Vector3 = make_idl_struct( "Vector3", - "geometry_msgs/msg/Vector3", + "espp_examples/msg/Vector3", {"x": float64, "y": float64, "z": float64}, ) -# A composite message showing the pieces you actually hit in real message types: -# a string, scalars, a NESTED struct, a FIXED array, and a variable SEQUENCE. -Imu = make_idl_struct( - "Imu", - "sensor_msgs/msg/Imu", +# A composite demo message showing the pieces you actually hit in real message +# types: a string, scalars, a NESTED struct, a FIXED array, and a variable +# SEQUENCE. Demo type name (espp_examples/msg/SensorSample) so it never collides +# with an official ROS 2 schema. +SensorSample = make_idl_struct( + "SensorSample", + "espp_examples/msg/SensorSample", { "frame_id": str, # string "stamp_ns": uint64, # scalar diff --git a/python/rtps_publisher.py b/python/rtps_publisher.py index 9e54b8d99..f63e36f76 100644 --- a/python/rtps_publisher.py +++ b/python/rtps_publisher.py @@ -1,7 +1,7 @@ #!/usr/bin/env python3 """Standalone RTPS publisher using the espp Python library (embeddedRTPS engine). -Publishes a complex nested message (sensor_msgs/msg/Imu-style: string + scalars + +Publishes a complex nested message (espp_examples/msg/SensorSample demo message: string + scalars + nested struct + fixed array + variable sequence) using pycdr2 for CDR (de)serialization and the light espp.rtps.Publisher wrapper. Pair it with rtps_subscriber.py, a FastDDS peer, or a ROS 2 node (rmw_fastrtps). @@ -16,7 +16,7 @@ import espp -from rtps_messages import Imu, Vector3 +from rtps_messages import SensorSample, Vector3 def main() -> int: @@ -30,18 +30,18 @@ def main() -> int: print("failed to start participant") return 1 - # Publisher: the DDS type name is derived from the pycdr2 typename - # ("sensor_msgs/msg/Imu" -> "sensor_msgs::msg::dds_::Imu_"). - pub = espp.rtps.Publisher(participant, topic, Imu, reliable=True) + # Publisher: the DDS type name is derived from the pycdr2 typename + # ("espp_examples/msg/SensorSample" -> "espp_examples::msg::dds_::SensorSample_"). + pub = espp.rtps.Publisher(participant, topic, SensorSample, reliable=True) if not pub.valid: print("failed to add writer") return 1 - print(f"publishing '{topic}' (sensor_msgs/msg/Imu) every {period}s; ctrl-c to stop") + print(f"publishing '{topic}' (espp_examples/msg/SensorSample) every {period}s; ctrl-c to stop") count = 0 try: while True: - msg = Imu( + msg = SensorSample( frame_id="imu_link", stamp_ns=time.time_ns(), seq=count, diff --git a/python/rtps_subscriber.py b/python/rtps_subscriber.py index 121753030..1bda73845 100644 --- a/python/rtps_subscriber.py +++ b/python/rtps_subscriber.py @@ -2,7 +2,7 @@ """Standalone RTPS subscriber using the espp Python library (embeddedRTPS engine). Subscribes to the complex nested message published by rtps_publisher.py (or any -DDS/ROS 2 peer publishing sensor_msgs/msg/Imu), using pycdr2 for CDR +DDS/ROS 2 peer publishing espp_examples/msg/SensorSample), using pycdr2 for CDR deserialization and the light espp.rtps.Subscriber wrapper -- the callback receives fully-decoded message objects, not raw bytes. @@ -16,7 +16,7 @@ import espp -from rtps_messages import Imu +from rtps_messages import SensorSample def main() -> int: @@ -26,7 +26,7 @@ def main() -> int: stats = {"received": 0} - def on_message(msg) -> None: # msg is a fully-decoded Imu + def on_message(msg) -> None: # msg is a fully-decoded SensorSample stats["received"] += 1 print( f"received {stats['received']}: seq={msg.seq} frame={msg.frame_id!r} " @@ -38,11 +38,11 @@ def on_message(msg) -> None: # msg is a fully-decoded Imu if not participant.start(): print("failed to start participant") return 1 - sub = espp.rtps.Subscriber(participant, topic, Imu, on_message=on_message, reliable=True) + sub = espp.rtps.Subscriber(participant, topic, SensorSample, on_message=on_message, reliable=True) if not sub.valid: print("failed to add reader") return 1 - print(f"subscribed to '{topic}' (sensor_msgs/msg/Imu) for {run_seconds}s") + print(f"subscribed to '{topic}' (espp_examples/msg/SensorSample) for {run_seconds}s") try: time.sleep(run_seconds) diff --git a/python/rtps_typed_test.py b/python/rtps_typed_test.py index 7f0e2f40d..5a25b0caa 100644 --- a/python/rtps_typed_test.py +++ b/python/rtps_typed_test.py @@ -15,7 +15,7 @@ import espp -from rtps_messages import Imu, Vector3 +from rtps_messages import SensorSample, Vector3 REQUIRED = 5 TOPIC = "rt/imu_test" @@ -36,8 +36,8 @@ def on_message(msg) -> None: print("FAIL: start") return 1 - pub = espp.rtps.Publisher(pub_p, TOPIC, Imu, reliable=True) - sub = espp.rtps.Subscriber(sub_p, TOPIC, Imu, on_message=on_message, reliable=True) + pub = espp.rtps.Publisher(pub_p, TOPIC, SensorSample, reliable=True) + sub = espp.rtps.Subscriber(sub_p, TOPIC, SensorSample, on_message=on_message, reliable=True) if not pub.valid or not sub.valid: print("FAIL: registration") return 1 @@ -48,7 +48,7 @@ def on_message(msg) -> None: deadline = time.monotonic() + 20.0 while len(received) < REQUIRED and time.monotonic() < deadline: pub.publish( - Imu( + SensorSample( frame_id="imu_link", stamp_ns=1000 + sent, seq=sent,