feat(LibCarla/ros2): [1/3] add publisher durability QoS and latched OpenDRIVE map topic

LibCarla/source/carla/ros2/ROS2.cpp

@@ -27,6 +27,7 @@
 #include "publishers/CarlaIMUPublisher.h"
 #include "publishers/CarlaISCameraPublisher.h"
 #include "publishers/CarlaLidarPublisher.h"
+#include "publishers/CarlaMapPublisher.h"
 #include "publishers/CarlaNormalsCameraPublisher.h"
 #include "publishers/CarlaOpticalFlowCameraPublisher.h"
 #include "publishers/CarlaRadarPublisher.h"
@@ -478,6 +479,24 @@ void ROS2::ProcessDataFromCollisionSensor(
   }
 }
 
+void ROS2::ProcessDataFromMap(const std::string &open_drive) {
+  std::lock_guard<std::recursive_mutex> lock(_mutex);
+  if (!_enabled) {
+    return;
+  }
+  if (open_drive.empty()) {
+    // Reached once per episode start, never per frame, so logging
+    // unconditionally cannot flood the output.
+    log_warning("ROS2: empty OpenDRIVE description, skipping map publish");
+    return;
+  }
+  if (!_map_publisher) {
+    _map_publisher = std::make_shared<CarlaMapPublisher>();
+  }
+  _map_publisher->Write(open_drive);
+  _map_publisher->Publish();
+}
+
 void ROS2::Shutdown() {
   std::lock_guard<std::recursive_mutex> lock(_mutex);
   // Destroy publishers first so DataWriter unregister-dispose messages are
@@ -488,6 +507,7 @@ void ROS2::Shutdown() {
   _publishers.clear();
   _tf_publishers.clear();
   _clock_publisher.reset();
+  _map_publisher.reset();
 
   _subscribers.clear();
 

LibCarla/source/carla/ros2/ROS2.h

@@ -44,6 +44,7 @@ namespace ros2 {
 
   class CarlaTransformPublisher;
   class CarlaClockPublisher;
+  class CarlaMapPublisher;
 
 class ROS2
 {
@@ -140,6 +141,10 @@ class ROS2
       uint32_t other_actor,
       carla::geom::Vector3D impulse,
       void* actor);
+    // Publishes the OpenDRIVE description of the current map as a latched
+    // topic. Called once per episode; re-publishing refreshes the latched
+    // sample after a map change.
+    void ProcessDataFromMap(const std::string &open_drive);
 
   private:
     std::shared_ptr<CarlaTransformPublisher> GetOrCreateTransformPublisher(void *actor);
@@ -162,6 +167,7 @@ class ROS2
   uint32_t _nanoseconds { 0 };
 
   std::shared_ptr<CarlaClockPublisher> _clock_publisher;
+  std::shared_ptr<CarlaMapPublisher> _map_publisher;
 
   // actor->parent relationship
   std::unordered_map<void *, void *> _actor_parent_map;

LibCarla/source/carla/ros2/middleware/IPublisherMiddleware.h

@@ -4,6 +4,8 @@
 
 #pragma once
 
+#include "carla/ros2/middleware/PublisherQos.h"
+
 #include <string>
 
 namespace carla {
@@ -23,9 +25,13 @@ class IPublisherMiddleware {
   virtual ~IPublisherMiddleware() = default;
 
   /// Initialize the underlying middleware entities.
-  /// @param topic_name  Full topic name including "rt/" prefix.
+  /// @param topic_name     Full topic name including "rt/" prefix.
+  /// @param publisher_qos  QoS applied to the underlying writer; the default
+  ///                       PublisherQos reproduces the historical behavior.
   /// @return true on success.
-  virtual bool Init(const std::string& topic_name) = 0;
+  virtual bool Init(
+      const std::string& topic_name,
+      const PublisherQos& publisher_qos) = 0;
 
   /// Serialize and write a message to the network.
   /// @param message_data  Pointer to the message object (type-erased, cast internally).

LibCarla/source/carla/ros2/middleware/PublisherQos.h

@@ -0,0 +1,57 @@
+// Copyright (c) 2026 Computer Vision Center (CVC) at the Universitat Autonoma de Barcelona (UAB).
+// This work is licensed under the terms of the MIT license.
+// For a copy, see <https://opensource.org/licenses/MIT>.
+
+#pragma once
+
+#include <cstdint>
+
+namespace carla {
+namespace ros2 {
+
+/// Durability of a publisher endpoint, mirroring the DDS durability QoS.
+enum class DurabilityKind : uint8_t {
+  /// Late-joining subscribers only receive samples written after they match.
+  Volatile,
+  /// The writer keeps a sample cache so late-joining subscribers receive the
+  /// last history_depth samples (the ROS 2 "latched topic" behavior).
+  TransientLocal
+};
+
+/// Reliability of a publisher endpoint, mirroring the DDS reliability QoS.
+enum class ReliabilityKind : uint8_t {
+  /// The writer retransmits until every matched reliable subscriber acks the
+  /// sample, blocking the publish call while the writer history is full.
+  Reliable,
+  /// Samples are sent once and may be dropped; the publish call never blocks
+  /// on slow subscribers (the ROS 2 sensor-data idiom for high-rate streams).
+  BestEffort
+};
+
+/// QoS settings applied to a publisher middleware at Init time.
+/// The defaults reproduce the exact behavior every publisher had before QoS
+/// support was added: reliable delivery, volatile durability, and a
+/// keep-last history of 1.
+struct PublisherQos {
+  DurabilityKind durability{DurabilityKind::Volatile};
+  ReliabilityKind reliability{ReliabilityKind::Reliable};
+  uint32_t history_depth{1u};
+
+  /// Profile for high-rate sensor streams (camera images, point clouds):
+  /// best-effort delivery so a slow or vanished subscriber can never stall
+  /// the publishing thread, matching the ROS 2 sensor-data QoS reliability.
+  static PublisherQos SensorData() {
+    PublisherQos qos;
+    qos.reliability = ReliabilityKind::BestEffort;
+    return qos;
+  }
+
+  /// History depth to hand to the middleware: a keep-last history needs a
+  /// positive depth, so the invalid value 0 is clamped to 1.
+  uint32_t effective_history_depth() const {
+    return history_depth == 0u ? 1u : history_depth;
+  }
+};
+
+} // namespace ros2
+} // namespace carla

LibCarla/source/carla/ros2/middleware/cyclonedds/CycloneDDSPublisherMiddleware.h

@@ -55,7 +55,9 @@ class CycloneDDSPublisherMiddleware : public IPublisherMiddleware {
     if (_topic  > 0) { dds_delete(_topic);  }
   }
 
-  bool Init(const std::string& topic_name) override {
+  bool Init(
+      const std::string& topic_name,
+      const PublisherQos& publisher_qos) override {
     dds_entity_t participant = carla_cdr_get_participant();
     if (participant < 0) {
       log_error("CycloneDDSPublisherMiddleware: Shared participant unavailable "
@@ -73,9 +75,35 @@ class CycloneDDSPublisherMiddleware : public IPublisherMiddleware {
     }
 
     dds_qos_t* qos = dds_create_qos();
-    dds_qset_reliability(qos, DDS_RELIABILITY_RELIABLE,
-                         DDS_SECS(1));
-    dds_qset_history(qos, DDS_HISTORY_KEEP_LAST, 1);
+    if (publisher_qos.reliability == ReliabilityKind::BestEffort) {
+      // High-rate sensor stream: drop samples instead of blocking the publish
+      // call on retransmissions to slow subscribers. max_blocking_time is
+      // meaningless for best-effort writers, so pass 0.
+      dds_qset_reliability(qos, DDS_RELIABILITY_BEST_EFFORT, 0);
+    } else {
+      dds_qset_reliability(qos, DDS_RELIABILITY_RELIABLE,
+                           DDS_SECS(1));
+    }
+    // Keep-last writer history with the requested depth. The default depth 1
+    // matches the previous hardcoded value, so volatile publishers are
+    // unchanged.
+    const int32_t depth =
+        static_cast<int32_t>(publisher_qos.effective_history_depth());
+    dds_qset_history(qos, DDS_HISTORY_KEEP_LAST, depth);
+    if (publisher_qos.durability == DurabilityKind::TransientLocal) {
+      // Latched topic: keep the last history_depth samples for late joiners.
+      // durability_service governs what the writer's transient-local cache
+      // delivers to a late-joining reader.
+      dds_qset_durability(qos, DDS_DURABILITY_TRANSIENT_LOCAL);
+      dds_qset_durability_service(
+          qos,
+          0,
+          DDS_HISTORY_KEEP_LAST,
+          depth,
+          DDS_LENGTH_UNLIMITED,
+          DDS_LENGTH_UNLIMITED,
+          DDS_LENGTH_UNLIMITED);
+    }
     // Set USER_DATA (PID_USER_DATA = 0x002c per OMG DDSI-RTPS v2.5 §9.6.2.2.2)
     // to the REP-2016 type-hash KV payload "typehash=RIHS01_<hex>;".
     auto ud = build_user_data_for<msg_type>();

LibCarla/source/carla/ros2/middleware/fastdds/FastDDSPublisherMiddleware.h

@@ -83,7 +83,9 @@ class FastDDSPublisherMiddleware
     }
   }
 
-  bool Init(const std::string& topic_name) override {
+  bool Init(
+      const std::string& topic_name,
+      const PublisherQos& publisher_qos) override {
     if (_type == nullptr) {
       log_error("FastDDSPublisherMiddleware: Invalid TypeSupport");
       return false;
@@ -125,6 +127,25 @@ class FastDDSPublisherMiddleware
     wqos.endpoint().history_memory_policy =
         eprosima::fastrtps::rtps::PREALLOCATED_WITH_REALLOC_MEMORY_MODE;
 
+    // Keep-last writer history with the requested depth. The default depth 1
+    // matches DATAWRITER_QOS_DEFAULT, so volatile publishers are unchanged.
+    wqos.history().kind = efd::KEEP_LAST_HISTORY_QOS;
+    wqos.history().depth =
+        static_cast<int32_t>(publisher_qos.effective_history_depth());
+
+    if (publisher_qos.durability == DurabilityKind::TransientLocal) {
+      // Latched topic: the writer keeps the last history_depth samples and
+      // hands them to late-joining subscribers that request transient_local.
+      wqos.durability().kind = efd::TRANSIENT_LOCAL_DURABILITY_QOS;
+    }
+
+    if (publisher_qos.reliability == ReliabilityKind::BestEffort) {
+      // High-rate sensor stream: drop samples instead of blocking the publish
+      // call on retransmissions to slow subscribers. The default stays
+      // RELIABLE (DATAWRITER_QOS_DEFAULT), unchanged for existing publishers.
+      wqos.reliability().kind = efd::BEST_EFFORT_RELIABILITY_QOS;
+    }
+
     // Set USER_DATA (PID_USER_DATA = 0x002c per OMG DDSI-RTPS v2.5 §9.6.2.2.2)
     // to the REP-2011/REP-2016 type-hash KV payload "typehash=RIHS01_<hex>;".
     // Jazzy rmw_cyclonedds_cpp / rmw_fastrtps_cpp parse this during SEDP