feat(rtps_embedded): Typed Publisher<T>/Subscriber<T> pub/sub (Phase 5)

.gitignore

@@ -55,3 +55,4 @@ managed_components/
 
 # docker interop harness build tree (bind-mounted, built in-container)
 pc/build-linux/
+python/env/

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<cdr::xcdr1>
-// emits the 4-byte encapsulation header + classic-CDR body ROS 2 speaks).
+// std_msgs/msg/String as a plain reflectable struct. The typed Publisher<T> /
+// Subscriber<T> 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<const uint8_t> u8_span(const std::vector<std::byte> &bytes) {
-  return {reinterpret_cast<const uint8_t *>(bytes.data()), bytes.size()};
-}
-
 extern "C" void app_main(void) {
   espp::Logger logger({.tag = "rtps_example", .level = espp::Logger::Verbosity::INFO});
 
@@ -58,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"); },
@@ -69,42 +66,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<const uint8_t> cdr_payload) {
-                if (auto msg = cdr::deserialize<StringMsg>(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;
+  espp::Publisher<StringMsg> publisher(participant, {
+                                                        .topic = pub_topic,
+                                                        .type_name = type_name,
+                                                        .reliability = Reliability::RELIABLE,
+                                                    });
+  // Typed subscriber: receive StringMsg structs directly.
+  espp::Subscriber<StringMsg> 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.
-  static uint32_t counter = 0;
+  // Publish a counter periodically via the typed publisher.
+  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<cdr::xcdr1>(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)");

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<const uint8_t> cdr_payload);
 
 protected:

components/rtps_embedded/include/rtps_pubsub.hpp

@@ -0,0 +1,197 @@
+#pragma once
+
+#include <concepts>
+#include <cstddef>
+#include <cstdint>
+#include <functional>
+#include <memory>
+#include <mutex>
+#include <span>
+#include <string>
+#include <string_view>
+#include <vector>
+
+#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 <typename T>
+concept RtpsMessage = requires(const T &value, std::span<const std::byte> bytes) {
+  { cdr::serialized_size<cdr::xcdr1>(value) } -> std::convertible_to<std::size_t>;
+  {cdr::deserialize<T>(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<Imu> 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/<name>" and type
+///       "<pkg>::msg::dds_::<Type>_" (e.g. "rt/chatter" +
+///       "std_msgs::msg::dds_::String_").
+template <RtpsMessage T> 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. 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, the
+  ///         writer history was full, or the serialized size exceeds
+  ///         RtpsParticipant::max_payload_size.
+  bool publish(const T &sample) {
+    if (!valid_) {
+      return false;
+    }
+    const std::size_t needed = cdr::serialized_size<cdr::xcdr1>(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<std::mutex> lock(mutex_);
+    if (buffer_.size() < needed) {
+      buffer_.resize(needed);
+    }
+    // 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<cdr::xcdr1>(sample, std::as_writable_bytes(std::span(buffer_)));
+    if (!written) {
+      return false;
+    }
+    return participant_->publish(topic_, std::span<const uint8_t>(buffer_.data(), *written));
+  }
+
+private:
+  RtpsParticipant *participant_{nullptr};
+  std::string topic_;
+  std::mutex mutex_;            ///< guards buffer_ against concurrent publish()
+  std::vector<uint8_t> 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<Imu> sub(participant, {.topic = "rt/imu",
+///                                         .type_name = "sensor_msgs::msg::dds_::Imu_",
+///                                         .on_message = [](const Imu &m) { use(m); }});
+/// @endcode
+template <RtpsMessage T> 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<void(const T &)>;
+
+  /// 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.
+  ///
+  /// \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_(std::make_shared<message_callback_t>(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 =
+            [callback](std::span<const uint8_t> cdr_payload) {
+              if (!*callback) {
+                return;
+              }
+              // std::as_bytes: the standard, well-defined span<uint8_t> ->
+              // span<const std::byte> conversion (no reinterpret_cast aliasing).
+              auto sample = cdr::deserialize<T>(std::as_bytes(cdr_payload));
+              if (sample) {
+                (*callback)(*sample);
+              }
+            },
+    });
+  }
+
+  /// \return True if the reader was registered successfully.
+  [[nodiscard]] bool is_valid() const { return valid_; }
+
+private:
+  std::shared_ptr<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

components/rtps_embedded/src/rtps_participant.cpp

@@ -1,6 +1,7 @@
 #include "rtps_participant.hpp"
 
 #include <cstdio>
+#include <limits>
 
 #include "rtps/entities/Domain.hpp"
 
@@ -245,6 +246,16 @@ bool RtpsParticipant::publish(std::string_view topic, std::span<const uint8_t> 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<rtps::DataSize_t>::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<rtps::DataSize_t>(cdr_payload.size()));
   if (change == nullptr) {

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"

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