feat(LibCarla/ros2): [2/7] add POD message types, FastDDS conversions, and TypeMap specializations

LibCarla/cmake/fast_dds/CMakeLists.txt

@@ -9,6 +9,7 @@ file(GLOB libcarla_carla_fastdds_headers
   "${libcarla_source_path}/carla/ros2/subscribers/*.h"
   "${libcarla_source_path}/carla/ros2/listeners/*.h"
   "${libcarla_source_path}/carla/ros2/types/*.h"
+  "${libcarla_source_path}/carla/ros2/types/msg/*.h"
   "${libcarla_source_path}/carla/ros2/dds/*.h"
   "${libcarla_source_path}/carla/ros2/dds/fastdds/*.h"
   )

LibCarla/source/carla/ros2/dds/DDSMiddleware.h

@@ -52,5 +52,17 @@ inline std::string GetAvailableMiddlewareString() {
   return result;
 }
 
+/// Mangle a DDS type name into the ROS2-compatible format.
+/// "sensor_msgs::msg::Image" becomes "sensor_msgs::msg::dds_::Image_".
+/// A bare name like "Image" becomes "dds_::Image_".
+inline std::string ToROS2DDSTypeName(const std::string& dds_type_name) {
+  auto pos = dds_type_name.rfind("::");
+  if (pos == std::string::npos) {
+    return "dds_::" + dds_type_name + "_";
+  }
+  return dds_type_name.substr(0, pos) +
+      "::dds_::" + dds_type_name.substr(pos + 2) + "_";
+}
+
 } // namespace ros2
 } // namespace carla

LibCarla/source/carla/ros2/dds/DDSMiddlewareFactory.h

@@ -12,7 +12,7 @@
 #include "carla/ros2/dds/IDDSSubscriberMiddleware.h"
 #include "carla/Logging.h"
 
-#if defined(CARLA_ROS2_DDS_FASTDDS)
+#if defined(CARLA_ROS2_DDS_FASTDDS) && !defined(CARLA_ROS2_DDS_TESTING)
 #  include "carla/ros2/dds/fastdds/FastDDSPublisherMiddleware.h"
 #  include "carla/ros2/dds/fastdds/FastDDSSubscriberMiddleware.h"
 #endif
@@ -77,7 +77,7 @@ class DDSMiddlewareFactory {
   static std::unique_ptr<IDDSPublisherMiddleware> CreatePublisher() {
     switch (GetActiveMiddleware()) {
       case DDSMiddleware::FastDDS:
-#if defined(CARLA_ROS2_DDS_FASTDDS)
+#if defined(CARLA_ROS2_DDS_FASTDDS) && !defined(CARLA_ROS2_DDS_TESTING)
         return std::unique_ptr<IDDSPublisherMiddleware>(
             new FastDDSPublisherMiddleware<T>());
 #else
@@ -95,7 +95,7 @@ class DDSMiddlewareFactory {
   static std::unique_ptr<IDDSSubscriberMiddleware> CreateSubscriber() {
     switch (GetActiveMiddleware()) {
       case DDSMiddleware::FastDDS:
-#if defined(CARLA_ROS2_DDS_FASTDDS)
+#if defined(CARLA_ROS2_DDS_FASTDDS) && !defined(CARLA_ROS2_DDS_TESTING)
         return std::unique_ptr<IDDSSubscriberMiddleware>(
             new FastDDSSubscriberMiddleware<S>());
 #else

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

@@ -2,22 +2,42 @@
 // This work is licensed under the terms of the MIT license.
 // For a copy, see <https://opensource.org/licenses/MIT>.
 
-// Identity-mapping version: msg_type IS the FastDDS type, so each
-// specialization maps a type to itself.
-
 #pragma once
 
+#include "carla/ros2/types/FastDDSConversions.h"
+
 // PubSubType headers for type registration
+#include "carla/ros2/types/TimePubSubTypes.h"
+#include "carla/ros2/types/HeaderPubSubTypes.h"
+#include "carla/ros2/types/Vector3PubSubTypes.h"
+#include "carla/ros2/types/QuaternionPubSubTypes.h"
+#include "carla/ros2/types/PointPubSubTypes.h"
+#include "carla/ros2/types/Point32PubSubTypes.h"
+#include "carla/ros2/types/PosePubSubTypes.h"
+#include "carla/ros2/types/PoseWithCovariancePubSubTypes.h"
+#include "carla/ros2/types/TwistPubSubTypes.h"
+#include "carla/ros2/types/TwistWithCovariancePubSubTypes.h"
+#include "carla/ros2/types/TransformPubSubTypes.h"
+#include "carla/ros2/types/TransformStampedPubSubTypes.h"
+#include "carla/ros2/types/OdometryPubSubTypes.h"
+#include "carla/ros2/types/RegionOfInterestPubSubTypes.h"
+#include "carla/ros2/types/PointFieldPubSubTypes.h"
+#include "carla/ros2/types/NavSatStatusPubSubTypes.h"
 #include "carla/ros2/types/NavSatFixPubSubTypes.h"
+#include "carla/ros2/types/ClockPubSubTypes.h"
+#include "carla/ros2/types/Float32PubSubTypes.h"
+#include "carla/ros2/types/StringPubSubTypes.h"
+#include "carla/ros2/types/ImuPubSubTypes.h"
 #include "carla/ros2/types/ImagePubSubTypes.h"
 #include "carla/ros2/types/CameraInfoPubSubTypes.h"
-#include "carla/ros2/types/ImuPubSubTypes.h"
 #include "carla/ros2/types/PointCloud2PubSubTypes.h"
-#include "carla/ros2/types/ClockPubSubTypes.h"
 #include "carla/ros2/types/TFMessagePubSubTypes.h"
+#include "carla/ros2/types/TF2ErrorPubSubTypes.h"
+#include "carla/ros2/types/AckermannDrivePubSubTypes.h"
+#include "carla/ros2/types/AckermannDriveStampedPubSubTypes.h"
 #include "carla/ros2/types/CarlaCollisionEventPubSubTypes.h"
 #include "carla/ros2/types/CarlaEgoVehicleControlPubSubTypes.h"
-#include "carla/ros2/types/AckermannDriveStampedPubSubTypes.h"
+#include "carla/ros2/types/CarlaLineInvasionPubSubTypes.h"
 
 namespace carla {
 namespace ros2 {
@@ -26,9 +46,11 @@ namespace ros2 {
 /// Primary template is intentionally undefined — only specializations are valid.
 template<typename MsgType> struct FastDDSTypeMap;
 
-// --- Identity specializations ---
-// In the current codebase, msg_type is already the FastDDS-generated type.
-// Each specialization maps the type to itself and its corresponding PubSubType.
+// ============================================================
+// Identity specializations (msg_type IS the FastDDS type)
+// Used by current publishers/subscribers until they are migrated
+// to POD message types in PR #2b.
+// ============================================================
 
 template<> struct FastDDSTypeMap<sensor_msgs::msg::NavSatFix> {
   using fastdds_type = sensor_msgs::msg::NavSatFix;
@@ -81,7 +103,7 @@ template<> struct FastDDSTypeMap<ackermann_msgs::msg::AckermannDriveStamped> {
 };
 
 /// Identity conversion: copy src to dst when both types are the same.
-/// In the next phase, we will replace these with real POD-to-FastDDS conversions.
+/// Used by publishers/subscribers that have not yet migrated to POD types.
 template<typename T>
 inline void to_fastdds(const T& src, T& dst) {
   dst = src;
@@ -92,5 +114,166 @@ inline void from_fastdds(const T& src, T& dst) {
   dst = src;
 }
 
+// ============================================================
+// Real specializations (POD msg types -> FastDDS types)
+// These map backend-neutral POD structs to FastDDS-generated types.
+// Conversion functions are in types/FastDDSConversions.h.
+// ============================================================
+
+template<> struct FastDDSTypeMap<msg::Time> {
+  using fastdds_type = builtin_interfaces::msg::Time;
+  using fastdds_pubsub_type = builtin_interfaces::msg::TimePubSubType;
+};
+
+template<> struct FastDDSTypeMap<msg::Header> {
+  using fastdds_type = std_msgs::msg::Header;
+  using fastdds_pubsub_type = std_msgs::msg::HeaderPubSubType;
+};
+
+template<> struct FastDDSTypeMap<msg::Vector3> {
+  using fastdds_type = geometry_msgs::msg::Vector3;
+  using fastdds_pubsub_type = geometry_msgs::msg::Vector3PubSubType;
+};
+
+template<> struct FastDDSTypeMap<msg::Quaternion> {
+  using fastdds_type = geometry_msgs::msg::Quaternion;
+  using fastdds_pubsub_type = geometry_msgs::msg::QuaternionPubSubType;
+};
+
+template<> struct FastDDSTypeMap<msg::Point> {
+  using fastdds_type = geometry_msgs::msg::Point;
+  using fastdds_pubsub_type = geometry_msgs::msg::PointPubSubType;
+};
+
+template<> struct FastDDSTypeMap<msg::Point32> {
+  using fastdds_type = geometry_msgs::msg::Point32;
+  using fastdds_pubsub_type = geometry_msgs::msg::Point32PubSubType;
+};
+
+template<> struct FastDDSTypeMap<msg::Pose> {
+  using fastdds_type = geometry_msgs::msg::Pose;
+  using fastdds_pubsub_type = geometry_msgs::msg::PosePubSubType;
+};
+
+template<> struct FastDDSTypeMap<msg::PoseWithCovariance> {
+  using fastdds_type = geometry_msgs::msg::PoseWithCovariance;
+  using fastdds_pubsub_type = geometry_msgs::msg::PoseWithCovariancePubSubType;
+};
+
+template<> struct FastDDSTypeMap<msg::Twist> {
+  using fastdds_type = geometry_msgs::msg::Twist;
+  using fastdds_pubsub_type = geometry_msgs::msg::TwistPubSubType;
+};
+
+template<> struct FastDDSTypeMap<msg::TwistWithCovariance> {
+  using fastdds_type = geometry_msgs::msg::TwistWithCovariance;
+  using fastdds_pubsub_type = geometry_msgs::msg::TwistWithCovariancePubSubType;
+};
+
+template<> struct FastDDSTypeMap<msg::Transform> {
+  using fastdds_type = geometry_msgs::msg::Transform;
+  using fastdds_pubsub_type = geometry_msgs::msg::TransformPubSubType;
+};
+
+template<> struct FastDDSTypeMap<msg::TransformStamped> {
+  using fastdds_type = geometry_msgs::msg::TransformStamped;
+  using fastdds_pubsub_type = geometry_msgs::msg::TransformStampedPubSubType;
+};
+
+template<> struct FastDDSTypeMap<msg::Odometry> {
+  using fastdds_type = nav_msgs::msg::Odometry;
+  using fastdds_pubsub_type = nav_msgs::msg::OdometryPubSubType;
+};
+
+template<> struct FastDDSTypeMap<msg::RegionOfInterest> {
+  using fastdds_type = sensor_msgs::msg::RegionOfInterest;
+  using fastdds_pubsub_type = sensor_msgs::msg::RegionOfInterestPubSubType;
+};
+
+template<> struct FastDDSTypeMap<msg::PointField> {
+  using fastdds_type = sensor_msgs::msg::PointField;
+  using fastdds_pubsub_type = sensor_msgs::msg::PointFieldPubSubType;
+};
+
+template<> struct FastDDSTypeMap<msg::NavSatStatus> {
+  using fastdds_type = sensor_msgs::msg::NavSatStatus;
+  using fastdds_pubsub_type = sensor_msgs::msg::NavSatStatusPubSubType;
+};
+
+template<> struct FastDDSTypeMap<msg::NavSatFix> {
+  using fastdds_type = sensor_msgs::msg::NavSatFix;
+  using fastdds_pubsub_type = sensor_msgs::msg::NavSatFixPubSubType;
+};
+
+template<> struct FastDDSTypeMap<msg::Clock> {
+  using fastdds_type = rosgraph::msg::Clock;
+  using fastdds_pubsub_type = rosgraph::msg::ClockPubSubType;
+};
+
+template<> struct FastDDSTypeMap<msg::Float32> {
+  using fastdds_type = std_msgs::msg::Float32;
+  using fastdds_pubsub_type = std_msgs::msg::Float32PubSubType;
+};
+
+template<> struct FastDDSTypeMap<msg::String> {
+  using fastdds_type = std_msgs::msg::String;
+  using fastdds_pubsub_type = std_msgs::msg::StringPubSubType;
+};
+
+template<> struct FastDDSTypeMap<msg::Imu> {
+  using fastdds_type = sensor_msgs::msg::Imu;
+  using fastdds_pubsub_type = sensor_msgs::msg::ImuPubSubType;
+};
+
+template<> struct FastDDSTypeMap<msg::Image> {
+  using fastdds_type = sensor_msgs::msg::Image;
+  using fastdds_pubsub_type = sensor_msgs::msg::ImagePubSubType;
+};
+
+template<> struct FastDDSTypeMap<msg::CameraInfo> {
+  using fastdds_type = sensor_msgs::msg::CameraInfo;
+  using fastdds_pubsub_type = sensor_msgs::msg::CameraInfoPubSubType;
+};
+
+template<> struct FastDDSTypeMap<msg::PointCloud2> {
+  using fastdds_type = sensor_msgs::msg::PointCloud2;
+  using fastdds_pubsub_type = sensor_msgs::msg::PointCloud2PubSubType;
+};
+
+template<> struct FastDDSTypeMap<msg::TFMessage> {
+  using fastdds_type = tf2_msgs::msg::TFMessage;
+  using fastdds_pubsub_type = tf2_msgs::msg::TFMessagePubSubType;
+};
+
+template<> struct FastDDSTypeMap<msg::TF2Error> {
+  using fastdds_type = tf2_msgs::msg::TF2Error;
+  using fastdds_pubsub_type = tf2_msgs::msg::TF2ErrorPubSubType;
+};
+
+template<> struct FastDDSTypeMap<msg::AckermannDrive> {
+  using fastdds_type = ackermann_msgs::msg::AckermannDrive;
+  using fastdds_pubsub_type = ackermann_msgs::msg::AckermannDrivePubSubType;
+};
+
+template<> struct FastDDSTypeMap<msg::AckermannDriveStamped> {
+  using fastdds_type = ackermann_msgs::msg::AckermannDriveStamped;
+  using fastdds_pubsub_type = ackermann_msgs::msg::AckermannDriveStampedPubSubType;
+};
+
+template<> struct FastDDSTypeMap<msg::CarlaCollisionEvent> {
+  using fastdds_type = carla_msgs::msg::CarlaCollisionEvent;
+  using fastdds_pubsub_type = carla_msgs::msg::CarlaCollisionEventPubSubType;
+};
+
+template<> struct FastDDSTypeMap<msg::CarlaEgoVehicleControl> {
+  using fastdds_type = carla_msgs::msg::CarlaEgoVehicleControl;
+  using fastdds_pubsub_type = carla_msgs::msg::CarlaEgoVehicleControlPubSubType;
+};
+
+template<> struct FastDDSTypeMap<msg::CarlaLineInvasion> {
+  using fastdds_type = carla_msgs::msg::LaneInvasionEvent;
+  using fastdds_pubsub_type = carla_msgs::msg::LaneInvasionEventPubSubType;
+};
+
 } // namespace ros2
 } // namespace carla

LibCarla/source/carla/ros2/publishers/PublisherImpl.h

@@ -19,11 +19,17 @@ namespace ros2 {
     using msg_type = typename T::msg_type;
 
     bool Init(std::string topic_name) {
-      _middleware = DDSMiddlewareFactory::CreatePublisher<T>();
+#ifdef LIBCARLA_WITH_GTEST
       if (!_middleware) {
-        log_error("PublisherImpl: Failed to create middleware publisher");
-        return false;
+#endif
+        _middleware = DDSMiddlewareFactory::CreatePublisher<T>();
+        if (!_middleware) {
+          log_error("PublisherImpl: Failed to create middleware publisher");
+          return false;
+        }
+#ifdef LIBCARLA_WITH_GTEST
       }
+#endif
       return _middleware->Init(topic_name);
     }
 
@@ -53,6 +59,12 @@ namespace ros2 {
       return _middleware->Publish(&_message);
     }
 
+#ifdef LIBCARLA_WITH_GTEST
+    void SetMiddlewareForTesting(std::unique_ptr<IDDSPublisherMiddleware> middleware) {
+      _middleware = std::move(middleware);
+    }
+#endif
+
   private:
     std::unique_ptr<IDDSPublisherMiddleware> _middleware;
     msg_type _message;

LibCarla/source/carla/ros2/subscribers/SubscriberImpl.h

@@ -20,11 +20,17 @@ namespace ros2 {
     using msg_type = typename S::msg_type;
 
     bool Init(std::string topic_name) {
-      _middleware = DDSMiddlewareFactory::CreateSubscriber<S>();
+#ifdef LIBCARLA_WITH_GTEST
       if (!_middleware) {
-        log_error("SubscriberImpl: Failed to create middleware subscriber");
-        return false;
+#endif
+        _middleware = DDSMiddlewareFactory::CreateSubscriber<S>();
+        if (!_middleware) {
+          log_error("SubscriberImpl: Failed to create middleware subscriber");
+          return false;
+        }
+#ifdef LIBCARLA_WITH_GTEST
       }
+#endif
       return _middleware->Init(topic_name, &_message, &_new_message);
     }
 
@@ -49,6 +55,17 @@ namespace ros2 {
 
     bool HasNewMessage() { return _new_message; }
 
+#ifdef LIBCARLA_WITH_GTEST
+    void SetMiddlewareForTesting(std::unique_ptr<IDDSSubscriberMiddleware> middleware) {
+      _middleware = std::move(middleware);
+    }
+
+    void SimulateMessageReceiptForTesting(const msg_type& msg) {
+      _message = msg;
+      _new_message = true;
+    }
+#endif
+
   private:
     std::unique_ptr<IDDSSubscriberMiddleware> _middleware;
     msg_type _message;