feat(LibCarla/ros2): [5/7] add unified CDR serialization and GenericCdrPubSubType

LibCarla/cmake/test/CMakeLists.txt

@@ -54,6 +54,17 @@ foreach(target ${build_targets})
   target_include_directories(${target} PRIVATE
       "${libcarla_source_path}/test")
 
+  # Server tests exercise CDR serialization (CdrSerialization.h) and
+  # GenericCdrPubSubType (inherits TopicDataType from fastrtps).
+  # Enable exceptions because Fast-CDR templates use try/catch internally.
+  if (CMAKE_BUILD_TYPE STREQUAL "Server")
+    target_include_directories(${target} SYSTEM PRIVATE "${FASTDDS_INCLUDE_PATH}")
+    target_compile_options(${target} PRIVATE -fexceptions)
+    target_link_libraries(${target} "${FASTDDS_LIB_PATH}/libfastrtps.a")
+    target_link_libraries(${target} "${FASTDDS_LIB_PATH}/libfastcdr.a")
+    target_link_libraries(${target} "${FASTDDS_LIB_PATH}/libfoonathan_memory-0.7.3.a")
+  endif()
+
   if (WIN32)
       target_link_libraries(${target} "gtest_main.lib")
       target_link_libraries(${target} "gtest.lib")

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

@@ -5,7 +5,7 @@
 #pragma once
 
 #include "carla/ros2/dds/IDDSPublisherMiddleware.h"
-#include "carla/ros2/dds/fastdds/FastDDSTypeMap.h"
+#include "carla/ros2/dds/fastdds/GenericCdrPubSubType.h"
 #include "carla/Logging.h"
 
 #include <fastdds/dds/domain/DomainParticipant.hpp>
@@ -30,17 +30,14 @@ using erc = eprosima::fastrtps::types::ReturnCode_t;
 
 /// FastDDS implementation of IDDSPublisherMiddleware.
 /// Parameterized on a traits type T that provides:
-///   T::msg_type  — the message type
-/// Native FastDDS types are resolved via FastDDSTypeMap<T::msg_type>.
+///   T::msg_type  — the message type (a carla::ros2::msg::* POD struct)
+/// Serialization is handled by GenericCdrPubSubType<msg_type> via CdrSerialization.h.
 template<typename T>
 class FastDDSPublisherMiddleware
     : public IDDSPublisherMiddleware,
       public eprosima::fastdds::dds::DataWriterListener {
  public:
   using msg_type = typename T::msg_type;
-  using type_map = FastDDSTypeMap<msg_type>;
-  using fastdds_type = typename type_map::fastdds_type;
-  using fastdds_pubsub_type = typename type_map::fastdds_pubsub_type;
 
   void on_publication_matched(
       efd::DataWriter* writer,
@@ -108,10 +105,8 @@ class FastDDSPublisherMiddleware
   }
 
   bool Publish(void* message_data) override {
-    auto* msg = static_cast<msg_type*>(message_data);
-    to_fastdds(*msg, _fastdds_msg);
     eprosima::fastrtps::rtps::InstanceHandle_t instance_handle;
-    erc rcode = _datawriter->write(&_fastdds_msg, instance_handle);
+    erc rcode = _datawriter->write(message_data, instance_handle);
     if (rcode == erc::ReturnCodeValue::RETCODE_OK) {
       return true;
     }
@@ -133,11 +128,10 @@ class FastDDSPublisherMiddleware
   efd::Publisher*         _publisher   { nullptr };
   efd::Topic*             _topic       { nullptr };
   efd::DataWriter*        _datawriter  { nullptr };
-  efd::TypeSupport        _type        { new fastdds_pubsub_type() };
+  efd::TypeSupport        _type        { new GenericCdrPubSubType<msg_type>() };
 
-  fastdds_type _fastdds_msg;
-  std::string  _topic_name;
-  bool         _alive { false };
+  std::string _topic_name;
+  bool        _alive { false };
 };
 
 } // namespace ros2

LibCarla/source/carla/ros2/dds/fastdds/FastDDSSubscriberMiddleware.h

@@ -5,7 +5,7 @@
 #pragma once
 
 #include "carla/ros2/dds/IDDSSubscriberMiddleware.h"
-#include "carla/ros2/dds/fastdds/FastDDSTypeMap.h"
+#include "carla/ros2/dds/fastdds/GenericCdrPubSubType.h"
 #include "carla/Logging.h"
 
 #include <fastdds/dds/domain/DomainParticipant.hpp>
@@ -31,17 +31,14 @@ using erc = eprosima::fastrtps::types::ReturnCode_t;
 
 /// FastDDS implementation of IDDSSubscriberMiddleware.
 /// Parameterized on traits type S that provides:
-///   S::msg_type  — the message type
-/// Native FastDDS types are resolved via FastDDSTypeMap<S::msg_type>.
+///   S::msg_type  — the message type (a carla::ros2::msg::* POD struct)
+/// Deserialization is handled by GenericCdrPubSubType<msg_type> via CdrSerialization.h.
 template<typename S>
 class FastDDSSubscriberMiddleware
     : public IDDSSubscriberMiddleware,
       public eprosima::fastdds::dds::DataReaderListener {
  public:
   using msg_type = typename S::msg_type;
-  using type_map = FastDDSTypeMap<msg_type>;
-  using fastdds_type = typename type_map::fastdds_type;
-  using fastdds_pubsub_type = typename type_map::fastdds_pubsub_type;
 
   void on_subscription_matched(
       efd::DataReader* reader,
@@ -51,9 +48,8 @@ class FastDDSSubscriberMiddleware
 
   void on_data_available(efd::DataReader* reader) override {
     efd::SampleInfo info;
-    erc rcode = reader->take_next_sample(&_fastdds_msg, &info);
+    erc rcode = reader->take_next_sample(_message_ptr, &info);
     if (rcode == erc::ReturnCodeValue::RETCODE_OK) {
-      from_fastdds(_fastdds_msg, *_message_ptr);
       *_new_message_ptr = true;
     } else {
       log_error("FastDDSSubscriberMiddleware::on_data_available (",
@@ -137,11 +133,10 @@ class FastDDSSubscriberMiddleware
   efd::Subscriber*        _subscriber  { nullptr };
   efd::Topic*             _topic       { nullptr };
   efd::DataReader*        _datareader  { nullptr };
-  efd::TypeSupport        _type        { new fastdds_pubsub_type() };
+  efd::TypeSupport        _type        { new GenericCdrPubSubType<msg_type>() };
 
-  fastdds_type _fastdds_msg;
-  msg_type*    _message_ptr     { nullptr };
-  bool*        _new_message_ptr { nullptr };
+  msg_type* _message_ptr     { nullptr };
+  bool*     _new_message_ptr { nullptr };
 
   std::string _topic_name;
   bool        _alive { false };

LibCarla/source/carla/ros2/dds/fastdds/GenericCdrPubSubType.h

@@ -0,0 +1,151 @@
+// Copyright (c) 2025 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 "carla/ros2/types/CdrSerialization.h"
+#include "carla/ros2/types/CdrTopicInfo.h"
+
+#include <fastdds/dds/topic/TopicDataType.hpp>
+#include <fastrtps/rtps/common/SerializedPayload.h>
+#include <fastcdr/FastBuffer.h>
+#include <fastcdr/Cdr.h>
+
+#include <cstdint>
+#include <cstring>
+#include <functional>
+
+namespace carla {
+namespace ros2 {
+
+/// Generic FastDDS TopicDataType that serializes carla::ros2::msg::* POD structs
+/// directly to CDR via CdrSerialization.h, without needing fastddsgen-generated
+/// per-type PubSubType classes.
+///
+/// Replaces all 30 hand-generated *PubSubType classes and FastDDSTypeMap<>.
+/// Type name and max size are provided by CdrTopicInfo<MsgType>.
+template<typename MsgType>
+class GenericCdrPubSubType : public eprosima::fastdds::dds::TopicDataType {
+ public:
+  using SerializedPayload_t = eprosima::fastrtps::rtps::SerializedPayload_t;
+
+  GenericCdrPubSubType() {
+    setName(CdrTopicInfo<MsgType>::type_name());
+    // m_typeSize is max CDR payload including the 4-byte DDS encapsulation header.
+    // FastDDS uses this to pre-allocate payload buffers.
+    const uint32_t max_payload = static_cast<uint32_t>(
+        CdrTopicInfo<MsgType>::max_serialized_size());
+    // Add alignment padding + 4-byte encapsulation header, matching the pattern
+    // in fastddsgen-generated constructors (e.g. ClockPubSubTypes.cpp:36-37).
+    m_typeSize = max_payload +
+        static_cast<uint32_t>(
+            eprosima::fastcdr::Cdr::alignment(max_payload, 4u)) +
+        4u;
+    m_isGetKeyDefined = false;
+  }
+
+  ~GenericCdrPubSubType() override = default;
+
+  /// Serialize a MsgType instance into the FastDDS payload buffer.
+  /// Called by FastDDS DataWriter::write() before sending on the wire.
+  /// Serializes into an auto-growing heap buffer first so variable-length
+  /// fields (e.g. Image::data, PointCloud2::data) are not bounded by the
+  /// pre-allocated payload->data size. The bytes are then memcpy'd across.
+  /// FastDDS resizes payload->data before this call via getSerializedSizeProvider,
+  /// so the copy will always fit for correctly sized messages.
+  bool serialize(
+      void* data,
+      SerializedPayload_t* payload) override {
+    const MsgType* msg = static_cast<const MsgType*>(data);
+
+    // Auto-growing FastBuffer: no fixed-size ceiling, handles any payload.
+    eprosima::fastcdr::FastBuffer fb;
+    // Force LITTLE_ENDIANNESS so the encapsulation header is CDR_LE
+    // ({0x00, 0x01}) per DDSI-RTPS v2.5 Table 10.3, regardless of host
+    // endianness. ROS2 ecosystems test against CDR_LE.
+    eprosima::fastcdr::Cdr ser(
+        fb,
+        eprosima::fastcdr::Cdr::LITTLE_ENDIANNESS,
+        eprosima::fastcdr::Cdr::DDS_CDR);
+    payload->encapsulation = CDR_LE;
+
+    try {
+      ser.serialize_encapsulation();
+      serialize_cdr(ser, *msg);
+    } catch (eprosima::fastcdr::exception::Exception& /*e*/) {
+      return false;
+    }
+
+    const uint32_t len = static_cast<uint32_t>(ser.getSerializedDataLength());
+    if (len > payload->max_size) {
+      return false;
+    }
+    std::memcpy(payload->data, fb.getBuffer(), len);
+    payload->length = len;
+    return true;
+  }
+
+  /// Deserialize a FastDDS payload buffer into a MsgType instance.
+  /// Called by FastDDS DataReader after receiving data from the wire.
+  bool deserialize(
+      SerializedPayload_t* payload,
+      void* data) override {
+    MsgType* msg = static_cast<MsgType*>(data);
+
+    eprosima::fastcdr::FastBuffer fastbuffer(
+        reinterpret_cast<char*>(payload->data),
+        static_cast<size_t>(payload->length));
+    // The deserializer must accept either endianness on the wire, the
+    // actual byte order is determined from the encapsulation header by
+    // read_encapsulation(). LITTLE_ENDIANNESS here is just the initial
+    // hint Fast-CDR uses before the header is parsed.
+    eprosima::fastcdr::Cdr deser(
+        fastbuffer,
+        eprosima::fastcdr::Cdr::LITTLE_ENDIANNESS,
+        eprosima::fastcdr::Cdr::DDS_CDR);
+
+    try {
+      deser.read_encapsulation();
+      payload->encapsulation = (deser.endianness() ==
+          eprosima::fastcdr::Cdr::BIG_ENDIANNESS) ? CDR_BE : CDR_LE;
+      deserialize_cdr(deser, *msg);
+    } catch (eprosima::fastcdr::exception::Exception& /*e*/) {
+      return false;
+    }
+
+    return true;
+  }
+
+  /// Return a function that gives the actual CDR-serialized size for this
+  /// specific message instance. FastDDS calls this before serialize() to
+  /// size (or resize) the payload buffer, so the buffer is always large enough
+  /// for variable-length fields like Image::data or PointCloud2::data.
+  std::function<uint32_t()> getSerializedSizeProvider(void* data) override {
+    const MsgType* msg = static_cast<const MsgType*>(data);
+    return [msg]() -> uint32_t {
+      return cdr_serialized_size(*msg);
+    };
+  }
+
+  /// Allocate a new default-initialized MsgType on the heap.
+  void* createData() override {
+    return static_cast<void*>(new MsgType());
+  }
+
+  /// Delete a MsgType previously returned by createData().
+  void deleteData(void* data) override {
+    delete static_cast<MsgType*>(data);
+  }
+
+  /// CARLA topics are not keyed — always return false.
+  bool getKey(
+      void* /*data*/,
+      eprosima::fastrtps::rtps::InstanceHandle_t* /*ihandle*/,
+      bool /*force_md5*/) override {
+    return false;
+  }
+};
+
+} // namespace ros2
+} // namespace carla