From 68a1402d31e5ab2be954f864ffd7a58fa411bcfc Mon Sep 17 00:00:00 2001 From: Jesus Armando Anaya Date: Mon, 6 Jul 2026 23:31:34 -0700 Subject: [PATCH 1/7] build(ros2): compile carla-ros2-native at C++20 The carla-ros2-native ExternalProject configures a separate CMake process, so it never inherited CMake/Common.cmake's CMAKE_CXX_STANDARD and compiled at the Clang default (C++17) while the rest of the project is C++20. Set the standard explicitly in the inner project and pass it through the ExternalProject CMAKE_ARGS so every consumer of the ROS 2 headers (carla-server, libcarla-ros2-native.so, the LibCarla test suites) builds at the same language level. Vendor ExternalProjects (fastdds, foonathan_memory) keep their own standard; only our code is raised. Validated: all TUs of the shared lib compile clean at -std=gnu++20 with 0 errors and 0 warnings under UE's libc++, and a full package-development build runs to BUILD SUCCESSFUL. FastDDS 2.11.2 and Fast-CDR 1.1 headers are C++20-clean. --- Ros2Native/CMakeLists.txt | 2 ++ Ros2Native/LibCarlaRos2Native/CMakeLists.txt | 3 +++ 2 files changed, 5 insertions(+) diff --git a/Ros2Native/CMakeLists.txt b/Ros2Native/CMakeLists.txt index 18b8d61a454..fb60dd25983 100644 --- a/Ros2Native/CMakeLists.txt +++ b/Ros2Native/CMakeLists.txt @@ -53,6 +53,8 @@ ExternalProject_Add ( -DCMAKE_BUILD_TYPE=${CMAKE_BUILD_TYPE} -DCMAKE_INSTALL_PREFIX=${PROJECT_INSTALL_PATH} -DCMAKE_TOOLCHAIN_FILE=${CMAKE_TOOLCHAIN_FILE} + -DCMAKE_CXX_STANDARD=20 + -DCMAKE_CXX_STANDARD_REQUIRED=ON ) set (CARLA_PLUGIN_BINARY_PATH ${CMAKE_SOURCE_DIR}/Unreal/CarlaUnreal/Plugins/Carla/Binaries/Linux) diff --git a/Ros2Native/LibCarlaRos2Native/CMakeLists.txt b/Ros2Native/LibCarlaRos2Native/CMakeLists.txt index f9d4c9e0f04..ecbd31ce0f7 100644 --- a/Ros2Native/LibCarlaRos2Native/CMakeLists.txt +++ b/Ros2Native/LibCarlaRos2Native/CMakeLists.txt @@ -7,6 +7,9 @@ project ( carla-ros2-native ) +set (CMAKE_CXX_STANDARD 20) +set (CMAKE_CXX_STANDARD_REQUIRED ON) + set ( LIBCARLA_SOURCE_PATH ${CMAKE_CURRENT_SOURCE_DIR}/../../LibCarla/source From 5d0d5786babdfb7b51ea4afd0a5a0cb58e77bd0b Mon Sep 17 00:00:00 2001 From: Jesus Armando Anaya Date: Mon, 6 Jul 2026 23:33:00 -0700 Subject: [PATCH 2/7] feat(ros2): add POD message types and CDR serialization foundation Introduce the middleware-neutral type layer of the ROS 2 middleware decoupling series, ported from ue4-dev: - types/msg/*.h: 31 plain C++ structs, one per ROS 2 message type, no DDS dependency, standard library headers only, all members value-initialized (upstream ue4-dev #9612). - types/CdrSerialization.h: serialize_to_cdr(), deserialize_from_cdr() and cdr_serialized_size() for all msg::* types using Fast-CDR (classic CDR, little-endian, DDS encapsulation header). The buffers are wire-compatible with every ROS 2 distribution and can be handed directly to FastDDS write() paths or CycloneDDS dds_writecdr(), removing the need for per-vendor generated type files. A kMaxCdrSequenceElements cap rejects hostile sequence lengths during deserialization (upstream ue4-dev #9643). - types/CdrTopicInfo.h: per-type type_name(), REP-2011 RIHS01 type hash and max_serialized_size(); the hashes let ROS 2 Iron and newer RMWs parse the type hash CARLA advertises via USER_DATA (upstream ue4-dev #9681). - types/UserDataFormat.h: build_user_data() / build_user_data_for() helpers producing the REP-2016 "typehash=RIHS01_;" key-value payload (upstream ue4-dev #9681). UE5 adaptation: ue5-dev pins FastDDS 2.11.2 with bundled Fast-CDR 1.x, so CdrSerialization.h keeps the Fast-CDR 1.x spellings (eprosima::fastcdr::Cdr::DDS_CDR, getSerializedDataLength()) instead of the Fast-CDR 2.x forms the ue4-dev tip carries since its Fast-DDS 2.14.6 upgrade (ue4-dev #9789). Five lines differ; the wire format is identical either way and is pinned by the golden-bytes test added in the follow-up test commit. The FastDDSConversions.h / FastDDSTypeMap.h files from #9612 are deliberately not ported; they were superseded by unified CDR upstream. The new headers are not referenced by any build target yet; they start compiling when the middleware abstraction lands in the next PR of the series. (adapted from ue4-dev 542959a51951dd35b93f5485155c5735056c017f) (adapted from ue4-dev f53144c97f5f1225606b9d4f8be4e35af1c7d5f8) (adapted from ue4-dev c64e8f49d44b35f9e1782caaea546a361600698b) (adapted from ue4-dev b8650880de0c0c9ec9e9dffc149fdae33094ca02) --- .../carla/ros2/types/CdrSerialization.h | 715 ++++++++++++++++++ .../source/carla/ros2/types/CdrTopicInfo.h | 393 ++++++++++ .../source/carla/ros2/types/UserDataFormat.h | 64 ++ .../carla/ros2/types/msg/AckermannDrive.h | 21 + .../ros2/types/msg/AckermannDriveStamped.h | 20 + .../source/carla/ros2/types/msg/CameraInfo.h | 33 + .../ros2/types/msg/CarlaCollisionEvent.h | 22 + .../ros2/types/msg/CarlaEgoVehicleControl.h | 26 + .../carla/ros2/types/msg/CarlaLineInvasion.h | 21 + LibCarla/source/carla/ros2/types/msg/Clock.h | 18 + .../source/carla/ros2/types/msg/Float32.h | 17 + LibCarla/source/carla/ros2/types/msg/Header.h | 20 + LibCarla/source/carla/ros2/types/msg/Image.h | 27 + LibCarla/source/carla/ros2/types/msg/Imu.h | 27 + .../source/carla/ros2/types/msg/NavSatFix.h | 32 + .../carla/ros2/types/msg/NavSatStatus.h | 28 + .../source/carla/ros2/types/msg/Odometry.h | 24 + LibCarla/source/carla/ros2/types/msg/Point.h | 19 + .../source/carla/ros2/types/msg/Point32.h | 19 + .../source/carla/ros2/types/msg/PointCloud2.h | 29 + .../source/carla/ros2/types/msg/PointField.h | 31 + LibCarla/source/carla/ros2/types/msg/Pose.h | 20 + .../carla/ros2/types/msg/PoseWithCovariance.h | 20 + .../source/carla/ros2/types/msg/Quaternion.h | 20 + .../carla/ros2/types/msg/RegionOfInterest.h | 22 + LibCarla/source/carla/ros2/types/msg/String.h | 18 + .../source/carla/ros2/types/msg/TF2Error.h | 20 + .../source/carla/ros2/types/msg/TFMessage.h | 19 + LibCarla/source/carla/ros2/types/msg/Time.h | 19 + .../source/carla/ros2/types/msg/Transform.h | 20 + .../carla/ros2/types/msg/TransformStamped.h | 22 + LibCarla/source/carla/ros2/types/msg/Twist.h | 19 + .../ros2/types/msg/TwistWithCovariance.h | 20 + .../source/carla/ros2/types/msg/Vector3.h | 19 + 34 files changed, 1864 insertions(+) create mode 100644 LibCarla/source/carla/ros2/types/CdrSerialization.h create mode 100644 LibCarla/source/carla/ros2/types/CdrTopicInfo.h create mode 100644 LibCarla/source/carla/ros2/types/UserDataFormat.h create mode 100644 LibCarla/source/carla/ros2/types/msg/AckermannDrive.h create mode 100644 LibCarla/source/carla/ros2/types/msg/AckermannDriveStamped.h create mode 100644 LibCarla/source/carla/ros2/types/msg/CameraInfo.h create mode 100644 LibCarla/source/carla/ros2/types/msg/CarlaCollisionEvent.h create mode 100644 LibCarla/source/carla/ros2/types/msg/CarlaEgoVehicleControl.h create mode 100644 LibCarla/source/carla/ros2/types/msg/CarlaLineInvasion.h create mode 100644 LibCarla/source/carla/ros2/types/msg/Clock.h create mode 100644 LibCarla/source/carla/ros2/types/msg/Float32.h create mode 100644 LibCarla/source/carla/ros2/types/msg/Header.h create mode 100644 LibCarla/source/carla/ros2/types/msg/Image.h create mode 100644 LibCarla/source/carla/ros2/types/msg/Imu.h create mode 100644 LibCarla/source/carla/ros2/types/msg/NavSatFix.h create mode 100644 LibCarla/source/carla/ros2/types/msg/NavSatStatus.h create mode 100644 LibCarla/source/carla/ros2/types/msg/Odometry.h create mode 100644 LibCarla/source/carla/ros2/types/msg/Point.h create mode 100644 LibCarla/source/carla/ros2/types/msg/Point32.h create mode 100644 LibCarla/source/carla/ros2/types/msg/PointCloud2.h create mode 100644 LibCarla/source/carla/ros2/types/msg/PointField.h create mode 100644 LibCarla/source/carla/ros2/types/msg/Pose.h create mode 100644 LibCarla/source/carla/ros2/types/msg/PoseWithCovariance.h create mode 100644 LibCarla/source/carla/ros2/types/msg/Quaternion.h create mode 100644 LibCarla/source/carla/ros2/types/msg/RegionOfInterest.h create mode 100644 LibCarla/source/carla/ros2/types/msg/String.h create mode 100644 LibCarla/source/carla/ros2/types/msg/TF2Error.h create mode 100644 LibCarla/source/carla/ros2/types/msg/TFMessage.h create mode 100644 LibCarla/source/carla/ros2/types/msg/Time.h create mode 100644 LibCarla/source/carla/ros2/types/msg/Transform.h create mode 100644 LibCarla/source/carla/ros2/types/msg/TransformStamped.h create mode 100644 LibCarla/source/carla/ros2/types/msg/Twist.h create mode 100644 LibCarla/source/carla/ros2/types/msg/TwistWithCovariance.h create mode 100644 LibCarla/source/carla/ros2/types/msg/Vector3.h diff --git a/LibCarla/source/carla/ros2/types/CdrSerialization.h b/LibCarla/source/carla/ros2/types/CdrSerialization.h new file mode 100644 index 00000000000..dcc90801651 --- /dev/null +++ b/LibCarla/source/carla/ros2/types/CdrSerialization.h @@ -0,0 +1,715 @@ +// 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 . + +#pragma once + +#include +#include +#include +#include + +#include +#include + +#include "carla/ros2/types/msg/AckermannDrive.h" +#include "carla/ros2/types/msg/AckermannDriveStamped.h" +#include "carla/ros2/types/msg/CameraInfo.h" +#include "carla/ros2/types/msg/CarlaCollisionEvent.h" +#include "carla/ros2/types/msg/CarlaEgoVehicleControl.h" +#include "carla/ros2/types/msg/CarlaLineInvasion.h" +#include "carla/ros2/types/msg/Clock.h" +#include "carla/ros2/types/msg/Float32.h" +#include "carla/ros2/types/msg/Header.h" +#include "carla/ros2/types/msg/Image.h" +#include "carla/ros2/types/msg/Imu.h" +#include "carla/ros2/types/msg/NavSatFix.h" +#include "carla/ros2/types/msg/NavSatStatus.h" +#include "carla/ros2/types/msg/Odometry.h" +#include "carla/ros2/types/msg/Point.h" +#include "carla/ros2/types/msg/Point32.h" +#include "carla/ros2/types/msg/PointCloud2.h" +#include "carla/ros2/types/msg/PointField.h" +#include "carla/ros2/types/msg/Pose.h" +#include "carla/ros2/types/msg/PoseWithCovariance.h" +#include "carla/ros2/types/msg/Quaternion.h" +#include "carla/ros2/types/msg/RegionOfInterest.h" +#include "carla/ros2/types/msg/String.h" +#include "carla/ros2/types/msg/TF2Error.h" +#include "carla/ros2/types/msg/TFMessage.h" +#include "carla/ros2/types/msg/Time.h" +#include "carla/ros2/types/msg/Transform.h" +#include "carla/ros2/types/msg/TransformStamped.h" +#include "carla/ros2/types/msg/Twist.h" +#include "carla/ros2/types/msg/TwistWithCovariance.h" +#include "carla/ros2/types/msg/Vector3.h" + +namespace carla { +namespace ros2 { + +// ========================================================================== +// Internal CDR helpers — one overload pair per message type. +// Ordered from least-dependent to most-dependent so each helper's body +// can call the helpers for its nested types without forward declarations. +// +// Wire format: OMG DDSI-RTPS v2.5 Section 10 + DDS-XTypes 1.3 clause 7.4.1.1 +// (Classic CDR, encoding version 1, little-endian). Sequences are encoded +// as a uint32_t length followed by elements; strings as a uint32_t length +// (including the terminating NUL) followed by bytes; bool as a single octet. +// ========================================================================== + +/// Sanity cap for the length field of a CDR sequence read from the wire. +/// Protects against malformed/hostile payloads claiming a multi-GB sequence, +/// which would otherwise OOM-abort the process inside std::vector::resize(). +/// 1,048,576 elements is far above any realistic ROS2 message (PointCloud2 +/// rarely has more than a dozen fields; TFMessage rarely has more than a few +/// hundred transforms) while bounding the worst-case allocation to ~80 MiB. +static constexpr uint32_t kMaxCdrSequenceElements = 1u << 20; + +// -------------------------------------------------------------------------- +// Leaf types (no nested msg:: fields) +// -------------------------------------------------------------------------- + +inline void serialize_cdr( + eprosima::fastcdr::Cdr& cdr, const msg::Time& m) { + cdr << m.sec; + cdr << m.nanosec; +} + +inline void deserialize_cdr( + eprosima::fastcdr::Cdr& cdr, msg::Time& m) { + cdr >> m.sec; + cdr >> m.nanosec; +} + +// -- + +inline void serialize_cdr( + eprosima::fastcdr::Cdr& cdr, const msg::Vector3& m) { + cdr << m.x; + cdr << m.y; + cdr << m.z; +} + +inline void deserialize_cdr( + eprosima::fastcdr::Cdr& cdr, msg::Vector3& m) { + cdr >> m.x; + cdr >> m.y; + cdr >> m.z; +} + +// -- + +inline void serialize_cdr( + eprosima::fastcdr::Cdr& cdr, const msg::Quaternion& m) { + cdr << m.x; + cdr << m.y; + cdr << m.z; + cdr << m.w; +} + +inline void deserialize_cdr( + eprosima::fastcdr::Cdr& cdr, msg::Quaternion& m) { + cdr >> m.x; + cdr >> m.y; + cdr >> m.z; + cdr >> m.w; +} + +// -- + +inline void serialize_cdr( + eprosima::fastcdr::Cdr& cdr, const msg::Point& m) { + cdr << m.x; + cdr << m.y; + cdr << m.z; +} + +inline void deserialize_cdr( + eprosima::fastcdr::Cdr& cdr, msg::Point& m) { + cdr >> m.x; + cdr >> m.y; + cdr >> m.z; +} + +// -- + +inline void serialize_cdr( + eprosima::fastcdr::Cdr& cdr, const msg::Point32& m) { + cdr << m.x; + cdr << m.y; + cdr << m.z; +} + +inline void deserialize_cdr( + eprosima::fastcdr::Cdr& cdr, msg::Point32& m) { + cdr >> m.x; + cdr >> m.y; + cdr >> m.z; +} + +// -- + +inline void serialize_cdr( + eprosima::fastcdr::Cdr& cdr, const msg::NavSatStatus& m) { + cdr << m.status; + cdr << m.service; +} + +inline void deserialize_cdr( + eprosima::fastcdr::Cdr& cdr, msg::NavSatStatus& m) { + cdr >> m.status; + cdr >> m.service; +} + +// -- + +inline void serialize_cdr( + eprosima::fastcdr::Cdr& cdr, const msg::RegionOfInterest& m) { + cdr << m.x_offset; + cdr << m.y_offset; + cdr << m.height; + cdr << m.width; + cdr << m.do_rectify; +} + +inline void deserialize_cdr( + eprosima::fastcdr::Cdr& cdr, msg::RegionOfInterest& m) { + cdr >> m.x_offset; + cdr >> m.y_offset; + cdr >> m.height; + cdr >> m.width; + cdr >> m.do_rectify; +} + +// -- + +inline void serialize_cdr( + eprosima::fastcdr::Cdr& cdr, const msg::Float32& m) { + cdr << m.data; +} + +inline void deserialize_cdr( + eprosima::fastcdr::Cdr& cdr, msg::Float32& m) { + cdr >> m.data; +} + +// -- + +inline void serialize_cdr( + eprosima::fastcdr::Cdr& cdr, const msg::AckermannDrive& m) { + cdr << m.steering_angle; + cdr << m.steering_angle_velocity; + cdr << m.speed; + cdr << m.acceleration; + cdr << m.jerk; +} + +inline void deserialize_cdr( + eprosima::fastcdr::Cdr& cdr, msg::AckermannDrive& m) { + cdr >> m.steering_angle; + cdr >> m.steering_angle_velocity; + cdr >> m.speed; + cdr >> m.acceleration; + cdr >> m.jerk; +} + +// -- + +/// PointField: name is a string; offset, datatype, count are primitives. +inline void serialize_cdr( + eprosima::fastcdr::Cdr& cdr, const msg::PointField& m) { + cdr << m.name; + cdr << m.offset; + cdr << m.datatype; + cdr << m.count; +} + +inline void deserialize_cdr( + eprosima::fastcdr::Cdr& cdr, msg::PointField& m) { + cdr >> m.name; + cdr >> m.offset; + cdr >> m.datatype; + cdr >> m.count; +} + +// -- + +inline void serialize_cdr( + eprosima::fastcdr::Cdr& cdr, const msg::TF2Error& m) { + cdr << m.error; + cdr << m.error_string; +} + +inline void deserialize_cdr( + eprosima::fastcdr::Cdr& cdr, msg::TF2Error& m) { + cdr >> m.error; + cdr >> m.error_string; +} + +// -------------------------------------------------------------------------- +// Types with nested msg:: fields +// -------------------------------------------------------------------------- + +inline void serialize_cdr( + eprosima::fastcdr::Cdr& cdr, const msg::Header& m) { + serialize_cdr(cdr, m.stamp); + cdr << m.frame_id; +} + +inline void deserialize_cdr( + eprosima::fastcdr::Cdr& cdr, msg::Header& m) { + deserialize_cdr(cdr, m.stamp); + cdr >> m.frame_id; +} + +// -- + +inline void serialize_cdr( + eprosima::fastcdr::Cdr& cdr, const msg::Twist& m) { + serialize_cdr(cdr, m.linear); + serialize_cdr(cdr, m.angular); +} + +inline void deserialize_cdr( + eprosima::fastcdr::Cdr& cdr, msg::Twist& m) { + deserialize_cdr(cdr, m.linear); + deserialize_cdr(cdr, m.angular); +} + +// -- + +inline void serialize_cdr( + eprosima::fastcdr::Cdr& cdr, const msg::Transform& m) { + serialize_cdr(cdr, m.translation); + serialize_cdr(cdr, m.rotation); +} + +inline void deserialize_cdr( + eprosima::fastcdr::Cdr& cdr, msg::Transform& m) { + deserialize_cdr(cdr, m.translation); + deserialize_cdr(cdr, m.rotation); +} + +// -- + +inline void serialize_cdr( + eprosima::fastcdr::Cdr& cdr, const msg::Pose& m) { + serialize_cdr(cdr, m.position); + serialize_cdr(cdr, m.orientation); +} + +inline void deserialize_cdr( + eprosima::fastcdr::Cdr& cdr, msg::Pose& m) { + deserialize_cdr(cdr, m.position); + deserialize_cdr(cdr, m.orientation); +} + +// -- + +inline void serialize_cdr( + eprosima::fastcdr::Cdr& cdr, const msg::Clock& m) { + serialize_cdr(cdr, m.clock); +} + +inline void deserialize_cdr( + eprosima::fastcdr::Cdr& cdr, msg::Clock& m) { + deserialize_cdr(cdr, m.clock); +} + +// -- + +inline void serialize_cdr( + eprosima::fastcdr::Cdr& cdr, const msg::String& m) { + cdr << m.data; +} + +inline void deserialize_cdr( + eprosima::fastcdr::Cdr& cdr, msg::String& m) { + cdr >> m.data; +} + +// -- + +inline void serialize_cdr( + eprosima::fastcdr::Cdr& cdr, const msg::TransformStamped& m) { + serialize_cdr(cdr, m.header); + cdr << m.child_frame_id; + serialize_cdr(cdr, m.transform); +} + +inline void deserialize_cdr( + eprosima::fastcdr::Cdr& cdr, msg::TransformStamped& m) { + deserialize_cdr(cdr, m.header); + cdr >> m.child_frame_id; + deserialize_cdr(cdr, m.transform); +} + +// -- + +inline void serialize_cdr( + eprosima::fastcdr::Cdr& cdr, const msg::TwistWithCovariance& m) { + serialize_cdr(cdr, m.twist); + cdr << m.covariance; +} + +inline void deserialize_cdr( + eprosima::fastcdr::Cdr& cdr, msg::TwistWithCovariance& m) { + deserialize_cdr(cdr, m.twist); + cdr >> m.covariance; +} + +// -- + +inline void serialize_cdr( + eprosima::fastcdr::Cdr& cdr, const msg::PoseWithCovariance& m) { + serialize_cdr(cdr, m.pose); + cdr << m.covariance; +} + +inline void deserialize_cdr( + eprosima::fastcdr::Cdr& cdr, msg::PoseWithCovariance& m) { + deserialize_cdr(cdr, m.pose); + cdr >> m.covariance; +} + +// -- + +inline void serialize_cdr( + eprosima::fastcdr::Cdr& cdr, const msg::AckermannDriveStamped& m) { + serialize_cdr(cdr, m.header); + serialize_cdr(cdr, m.drive); +} + +inline void deserialize_cdr( + eprosima::fastcdr::Cdr& cdr, msg::AckermannDriveStamped& m) { + deserialize_cdr(cdr, m.header); + deserialize_cdr(cdr, m.drive); +} + +// -- + +inline void serialize_cdr( + eprosima::fastcdr::Cdr& cdr, const msg::CarlaCollisionEvent& m) { + serialize_cdr(cdr, m.header); + cdr << m.other_actor_id; + serialize_cdr(cdr, m.normal_impulse); +} + +inline void deserialize_cdr( + eprosima::fastcdr::Cdr& cdr, msg::CarlaCollisionEvent& m) { + deserialize_cdr(cdr, m.header); + cdr >> m.other_actor_id; + deserialize_cdr(cdr, m.normal_impulse); +} + +// -- + +inline void serialize_cdr( + eprosima::fastcdr::Cdr& cdr, const msg::NavSatFix& m) { + serialize_cdr(cdr, m.header); + serialize_cdr(cdr, m.status); + cdr << m.latitude; + cdr << m.longitude; + cdr << m.altitude; + cdr << m.position_covariance; + cdr << m.position_covariance_type; +} + +inline void deserialize_cdr( + eprosima::fastcdr::Cdr& cdr, msg::NavSatFix& m) { + deserialize_cdr(cdr, m.header); + deserialize_cdr(cdr, m.status); + cdr >> m.latitude; + cdr >> m.longitude; + cdr >> m.altitude; + cdr >> m.position_covariance; + cdr >> m.position_covariance_type; +} + +// -- + +inline void serialize_cdr( + eprosima::fastcdr::Cdr& cdr, const msg::Imu& m) { + serialize_cdr(cdr, m.header); + serialize_cdr(cdr, m.orientation); + cdr << m.orientation_covariance; + serialize_cdr(cdr, m.angular_velocity); + cdr << m.angular_velocity_covariance; + serialize_cdr(cdr, m.linear_acceleration); + cdr << m.linear_acceleration_covariance; +} + +inline void deserialize_cdr( + eprosima::fastcdr::Cdr& cdr, msg::Imu& m) { + deserialize_cdr(cdr, m.header); + deserialize_cdr(cdr, m.orientation); + cdr >> m.orientation_covariance; + deserialize_cdr(cdr, m.angular_velocity); + cdr >> m.angular_velocity_covariance; + deserialize_cdr(cdr, m.linear_acceleration); + cdr >> m.linear_acceleration_covariance; +} + +// -- + +inline void serialize_cdr( + eprosima::fastcdr::Cdr& cdr, const msg::CarlaEgoVehicleControl& m) { + serialize_cdr(cdr, m.header); + cdr << m.throttle; + cdr << m.steer; + cdr << m.brake; + cdr << m.hand_brake; + cdr << m.reverse; + cdr << m.gear; + cdr << m.manual_gear_shift; +} + +inline void deserialize_cdr( + eprosima::fastcdr::Cdr& cdr, msg::CarlaEgoVehicleControl& m) { + deserialize_cdr(cdr, m.header); + cdr >> m.throttle; + cdr >> m.steer; + cdr >> m.brake; + cdr >> m.hand_brake; + cdr >> m.reverse; + cdr >> m.gear; + cdr >> m.manual_gear_shift; +} + +// -- + +inline void serialize_cdr( + eprosima::fastcdr::Cdr& cdr, const msg::CarlaLineInvasion& m) { + serialize_cdr(cdr, m.header); + cdr << m.crossed_lane_markings; +} + +inline void deserialize_cdr( + eprosima::fastcdr::Cdr& cdr, msg::CarlaLineInvasion& m) { + deserialize_cdr(cdr, m.header); + cdr >> m.crossed_lane_markings; +} + +// -- + +inline void serialize_cdr( + eprosima::fastcdr::Cdr& cdr, const msg::Odometry& m) { + serialize_cdr(cdr, m.header); + cdr << m.child_frame_id; + serialize_cdr(cdr, m.pose); + serialize_cdr(cdr, m.twist); +} + +inline void deserialize_cdr( + eprosima::fastcdr::Cdr& cdr, msg::Odometry& m) { + deserialize_cdr(cdr, m.header); + cdr >> m.child_frame_id; + deserialize_cdr(cdr, m.pose); + deserialize_cdr(cdr, m.twist); +} + +// -- + +inline void serialize_cdr( + eprosima::fastcdr::Cdr& cdr, const msg::Image& m) { + serialize_cdr(cdr, m.header); + cdr << m.height; + cdr << m.width; + cdr << m.encoding; + cdr << m.is_bigendian; + cdr << m.step; + cdr << m.data; +} + +inline void deserialize_cdr( + eprosima::fastcdr::Cdr& cdr, msg::Image& m) { + deserialize_cdr(cdr, m.header); + cdr >> m.height; + cdr >> m.width; + cdr >> m.encoding; + cdr >> m.is_bigendian; + cdr >> m.step; + cdr >> m.data; +} + +// -- + +inline void serialize_cdr( + eprosima::fastcdr::Cdr& cdr, const msg::CameraInfo& m) { + serialize_cdr(cdr, m.header); + cdr << m.height; + cdr << m.width; + cdr << m.distortion_model; + cdr << m.d; + cdr << m.k; + cdr << m.r; + cdr << m.p; + cdr << m.binning_x; + cdr << m.binning_y; + serialize_cdr(cdr, m.roi); +} + +inline void deserialize_cdr( + eprosima::fastcdr::Cdr& cdr, msg::CameraInfo& m) { + deserialize_cdr(cdr, m.header); + cdr >> m.height; + cdr >> m.width; + cdr >> m.distortion_model; + cdr >> m.d; + cdr >> m.k; + cdr >> m.r; + cdr >> m.p; + cdr >> m.binning_x; + cdr >> m.binning_y; + deserialize_cdr(cdr, m.roi); +} + +// -- + +/// PointCloud2::fields is a sequence of structs; FastCDR's generic vector +/// operator calls serialize() on each element, which our POD types don't +/// provide. Write length + elements manually. +inline void serialize_cdr( + eprosima::fastcdr::Cdr& cdr, const msg::PointCloud2& m) { + serialize_cdr(cdr, m.header); + cdr << m.height; + cdr << m.width; + // CDR sequence length is uint32_t per DDS-XTypes 1.3 clause 7.4.1.1. + cdr << static_cast(m.fields.size()); + for (const auto& f : m.fields) { + serialize_cdr(cdr, f); + } + cdr << m.is_bigendian; + cdr << m.point_step; + cdr << m.row_step; + cdr << m.data; + cdr << m.is_dense; +} + +inline void deserialize_cdr( + eprosima::fastcdr::Cdr& cdr, msg::PointCloud2& m) { + deserialize_cdr(cdr, m.header); + cdr >> m.height; + cdr >> m.width; + uint32_t fields_size{0u}; + cdr >> fields_size; + if (fields_size > kMaxCdrSequenceElements) { + throw eprosima::fastcdr::exception::BadParamException( + "PointCloud2::fields length exceeds sane CDR sequence cap"); + } + m.fields.resize(static_cast(fields_size)); + for (auto& f : m.fields) { + deserialize_cdr(cdr, f); + } + cdr >> m.is_bigendian; + cdr >> m.point_step; + cdr >> m.row_step; + cdr >> m.data; + cdr >> m.is_dense; +} + +// -- + +/// TFMessage::transforms is a sequence of TransformStamped structs. +/// Write length + elements manually. +inline void serialize_cdr( + eprosima::fastcdr::Cdr& cdr, const msg::TFMessage& m) { + // CDR sequence length is uint32_t per DDS-XTypes 1.3 clause 7.4.1.1. + cdr << static_cast(m.transforms.size()); + for (const auto& t : m.transforms) { + serialize_cdr(cdr, t); + } +} + +inline void deserialize_cdr( + eprosima::fastcdr::Cdr& cdr, msg::TFMessage& m) { + uint32_t transforms_size{0u}; + cdr >> transforms_size; + if (transforms_size > kMaxCdrSequenceElements) { + throw eprosima::fastcdr::exception::BadParamException( + "TFMessage::transforms length exceeds sane CDR sequence cap"); + } + m.transforms.resize(static_cast(transforms_size)); + for (auto& t : m.transforms) { + deserialize_cdr(cdr, t); + } +} + +// ========================================================================== +// Public API +// ========================================================================== + +/// Serialize a msg::X to a CDR byte buffer including the DDS encapsulation +/// header (Classic CDR, encoding version 1, little-endian). The returned +/// buffer is wire-compatible with all ROS2 distros and can be passed to +/// FastDDS write_serialized_payload() or CycloneDDS dds_writecdr(). +/// Returns an empty vector if Fast-CDR raises an exception (e.g. out of +/// memory while growing the internal FastBuffer). +template +std::vector serialize_to_cdr(const T& msg) { + 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 cdr{ + fb, + eprosima::fastcdr::Cdr::LITTLE_ENDIANNESS, + eprosima::fastcdr::Cdr::DDS_CDR}; + try { + cdr.serialize_encapsulation(); + serialize_cdr(cdr, msg); + } catch (const eprosima::fastcdr::exception::Exception&) { + return std::vector{}; + } + const char* buf{fb.getBuffer()}; + const size_t len{cdr.getSerializedDataLength()}; + return std::vector{ + reinterpret_cast(buf), + reinterpret_cast(buf) + len}; +} + +/// Return the exact CDR-serialized size in bytes for a message instance, +/// including the 4-byte DDS encapsulation header. Used by GenericCdrPubSubType +/// to tell FastDDS the actual payload size before write(), so the payload +/// buffer is sized correctly for variable-length fields (e.g. Image::data, +/// PointCloud2::data). The result is provably equal to serialize_to_cdr(msg).size(). +template +uint32_t cdr_serialized_size(const T& msg) { + eprosima::fastcdr::FastBuffer fb; + eprosima::fastcdr::Cdr cdr{ + fb, + eprosima::fastcdr::Cdr::LITTLE_ENDIANNESS, + eprosima::fastcdr::Cdr::DDS_CDR}; + cdr.serialize_encapsulation(); + serialize_cdr(cdr, msg); + return static_cast(cdr.getSerializedDataLength()); +} + +/// Deserialize a msg::X from a CDR byte buffer that was produced by +/// serialize_to_cdr() or by any ROS2-compatible DDS middleware. +/// Returns true on success, false on any Fast-CDR error (truncated buffer, +/// malformed encapsulation header, sequence length exceeding the sanity +/// cap, etc.). The buffer must include the 4-byte DDS encapsulation header. +template +bool deserialize_from_cdr( + const uint8_t* data, size_t size, T& msg) { + eprosima::fastcdr::FastBuffer fb{ + // FastBuffer requires a non-const pointer; the buffer is only read. + reinterpret_cast(const_cast(data)), + size}; + eprosima::fastcdr::Cdr cdr{ + fb, + eprosima::fastcdr::Cdr::LITTLE_ENDIANNESS, + eprosima::fastcdr::Cdr::DDS_CDR}; + try { + cdr.read_encapsulation(); + deserialize_cdr(cdr, msg); + } catch (const eprosima::fastcdr::exception::Exception&) { + return false; + } + return true; +} + +} // namespace ros2 +} // namespace carla diff --git a/LibCarla/source/carla/ros2/types/CdrTopicInfo.h b/LibCarla/source/carla/ros2/types/CdrTopicInfo.h new file mode 100644 index 00000000000..3df073efdaf --- /dev/null +++ b/LibCarla/source/carla/ros2/types/CdrTopicInfo.h @@ -0,0 +1,393 @@ +// 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 . + +#pragma once + +#include + +#include "carla/ros2/types/msg/AckermannDrive.h" +#include "carla/ros2/types/msg/AckermannDriveStamped.h" +#include "carla/ros2/types/msg/CameraInfo.h" +#include "carla/ros2/types/msg/CarlaCollisionEvent.h" +#include "carla/ros2/types/msg/CarlaEgoVehicleControl.h" +#include "carla/ros2/types/msg/CarlaLineInvasion.h" +#include "carla/ros2/types/msg/Clock.h" +#include "carla/ros2/types/msg/Float32.h" +#include "carla/ros2/types/msg/Header.h" +#include "carla/ros2/types/msg/Image.h" +#include "carla/ros2/types/msg/Imu.h" +#include "carla/ros2/types/msg/NavSatFix.h" +#include "carla/ros2/types/msg/NavSatStatus.h" +#include "carla/ros2/types/msg/Odometry.h" +#include "carla/ros2/types/msg/Point.h" +#include "carla/ros2/types/msg/Point32.h" +#include "carla/ros2/types/msg/PointCloud2.h" +#include "carla/ros2/types/msg/PointField.h" +#include "carla/ros2/types/msg/Pose.h" +#include "carla/ros2/types/msg/PoseWithCovariance.h" +#include "carla/ros2/types/msg/Quaternion.h" +#include "carla/ros2/types/msg/RegionOfInterest.h" +#include "carla/ros2/types/msg/String.h" +#include "carla/ros2/types/msg/TF2Error.h" +#include "carla/ros2/types/msg/TFMessage.h" +#include "carla/ros2/types/msg/Time.h" +#include "carla/ros2/types/msg/Transform.h" +#include "carla/ros2/types/msg/TransformStamped.h" +#include "carla/ros2/types/msg/Twist.h" +#include "carla/ros2/types/msg/TwistWithCovariance.h" +#include "carla/ros2/types/msg/Vector3.h" + +namespace carla { +namespace ros2 { + +/// Per-type metadata needed by the middleware layer. +/// +/// type_name() — ROS2-compatible type name string used when +/// registering the type with a DomainParticipant. +/// Follows the "pkg::msg::dds_::TypeName_" pattern. +/// +/// type_hash() — REP-2011 RIHS01 type hash string, placed in the +/// DDS endpoint USER_DATA QoS policy (PID_USER_DATA, +/// 0x002c per OMG DDSI-RTPS v2.5 §9.6.2.2.2). +/// Format: "RIHS01_<64 lowercase hex>" (71 chars). +/// Consumed by UserDataFormat.h to build the +/// REP-2016 KV payload "typehash=RIHS01_;". +/// The hash is pinned per message definition and is +/// stable across ROS 2 distributions. A specialization +/// may return nullptr when no hash is available for the +/// type; UserDataFormat.h then emits an empty user_data +/// payload and callers skip setting the QoS field. +/// To compute the hash for a new type, use +/// Util/ros2/compute_type_hash.sh (Docker required). +/// Full workflow: Docs/ros2/adding_message_types.md. +/// +/// max_serialized_size() — Initial preallocation hint for the CDR payload size +/// in bytes, excluding the 4-byte DDS encapsulation +/// header. Used by FastDDS to pre-allocate payload +/// buffers. For types with variable-length fields +/// (strings, vectors) this is a minimum hint, not a +/// hard limit. The actual size per message instance is +/// computed dynamically by cdr_serialized_size(). +/// +/// Primary template is intentionally undefined — only specializations are +/// valid. +template struct CdrTopicInfo; + +// ========================================================================== +// Specializations — ordered alphabetically by C++ type name +// ========================================================================== + +template<> struct CdrTopicInfo { + static const char* type_name() { + return "ackermann_msgs::msg::dds_::AckermannDrive_"; + } + static const char* type_hash() { + return "RIHS01_acf287a224a947dd1b0b87d6d76cdb73f497b0237b8fc73be2173b2ebbb82c99"; + } + static size_t max_serialized_size() { return 20u; } +}; + +template<> struct CdrTopicInfo { + static const char* type_name() { + return "ackermann_msgs::msg::dds_::AckermannDriveStamped_"; + } + static const char* type_hash() { + return "RIHS01_48ca7612a08d3bb72744fd98b71b7cf2ea24c6ad50fa4e1aa0bbad963c90d8cf"; + } + static size_t max_serialized_size() { return 288u; } +}; + +template<> struct CdrTopicInfo { + static const char* type_name() { + return "sensor_msgs::msg::dds_::CameraInfo_"; + } + static const char* type_hash() { + return "RIHS01_b3dfd68ff46c9d56c80fd3bd4ed22c7a4ddce8c8348f2f59c299e73118e7e275"; + } + static size_t max_serialized_size() { return 3793u; } +}; + +template<> struct CdrTopicInfo { + static const char* type_name() { + return "carla_msgs::msg::dds_::CarlaCollisionEvent_"; + } + static const char* type_hash() { + return "RIHS01_d77acb472c5effb98998bfb2e176ae3ffac0a84ce33a79f90e999544e3fcfeff"; + } + static size_t max_serialized_size() { return 296u; } +}; + +template<> struct CdrTopicInfo { + static const char* type_name() { + return "carla_msgs::msg::dds_::CarlaEgoVehicleControl_"; + } + static const char* type_hash() { + return "RIHS01_4f251fa2a554e8ed996f77eb1d5b65515af1369eceb04d5122cb5761f7801be3"; + } + static size_t max_serialized_size() { return 289u; } +}; + +template<> struct CdrTopicInfo { + static const char* type_name() { + return "carla_msgs::msg::dds_::LaneInvasionEvent_"; + } + static const char* type_hash() { + return "RIHS01_1d81c780738761101e1b4cb165af96ed32b8aa5cd523713c9b28d1ee95af719b"; + } + static size_t max_serialized_size() { return 672u; } +}; + +template<> struct CdrTopicInfo { + static const char* type_name() { + return "rosgraph_msgs::msg::dds_::Clock_"; + } + static const char* type_hash() { + return "RIHS01_692f7a66e93a3c83e71765d033b60349ba68023a8c689a79e48078bcb5c58564"; + } + // Clock holds one Time (8 bytes = 2 × int32). + static size_t max_serialized_size() { return 8u; } +}; + +template<> struct CdrTopicInfo { + static const char* type_name() { + return "std_msgs::msg::dds_::Float32_"; + } + static const char* type_hash() { + return "RIHS01_7170d3d8f841f7be3172ce5f4f59f3a4d7f63b0447e8b33327601ad64d83d6e2"; + } + static size_t max_serialized_size() { return 4u; } +}; + +template<> struct CdrTopicInfo { + static const char* type_name() { + return "std_msgs::msg::dds_::Header_"; + } + static const char* type_hash() { + return "RIHS01_f49fb3ae2cf070f793645ff749683ac6b06203e41c891e17701b1cb597ce6a01"; + } + static size_t max_serialized_size() { return 268u; } +}; + +template<> struct CdrTopicInfo { + static const char* type_name() { + return "sensor_msgs::msg::dds_::Image_"; + } + static const char* type_hash() { + return "RIHS01_d31d41a9a4c4bc8eae9be757b0beed306564f7526c88ea6a4588fb9582527d47"; + } + static size_t max_serialized_size() { return 648u; } +}; + +template<> struct CdrTopicInfo { + static const char* type_name() { + return "sensor_msgs::msg::dds_::Imu_"; + } + static const char* type_hash() { + return "RIHS01_7d9a00ff131080897a5ec7e26e315954b8eae3353c3f995c55faf71574000b5b"; + } + static size_t max_serialized_size() { return 568u; } +}; + +template<> struct CdrTopicInfo { + static const char* type_name() { + return "sensor_msgs::msg::dds_::NavSatFix_"; + } + static const char* type_hash() { + return "RIHS01_62223ab3fe210a15976021da7afddc9e200dc9ec75231c1b6a557fc598a65404"; + } + static size_t max_serialized_size() { return 369u; } +}; + +template<> struct CdrTopicInfo { + static const char* type_name() { + return "sensor_msgs::msg::dds_::NavSatStatus_"; + } + static const char* type_hash() { + return "RIHS01_d1ed3befa628e09571bd273b888ba1c1fd187c9a5e0006b385d7e5e9095a3204"; + } + static size_t max_serialized_size() { return 4u; } +}; + +template<> struct CdrTopicInfo { + static const char* type_name() { + return "nav_msgs::msg::dds_::Odometry_"; + } + static const char* type_hash() { + return "RIHS01_3cc97dc7fb7502f8714462c526d369e35b603cfc34d946e3f2eda2766dfec6e0"; + } + static size_t max_serialized_size() { return 1208u; } +}; + +template<> struct CdrTopicInfo { + static const char* type_name() { + return "geometry_msgs::msg::dds_::Point_"; + } + static const char* type_hash() { + return "RIHS01_6963084842a9b04494d6b2941d11444708d892da2f4b09843b9c43f42a7f6881"; + } + static size_t max_serialized_size() { return 24u; } +}; + +template<> struct CdrTopicInfo { + static const char* type_name() { + return "geometry_msgs::msg::dds_::Point32_"; + } + static const char* type_hash() { + return "RIHS01_2fc4db7cae16a4582c79a56b66173a8d48d52c7dc520ddc55a0d4bcf2a4bfdbc"; + } + static size_t max_serialized_size() { return 12u; } +}; + +template<> struct CdrTopicInfo { + static const char* type_name() { + return "sensor_msgs::msg::dds_::PointCloud2_"; + } + static const char* type_hash() { + return "RIHS01_9198cabf7da3796ae6fe19c4cb3bdd3525492988c70522628af5daa124bae2b5"; + } + static size_t max_serialized_size() { return 27597u; } +}; + +template<> struct CdrTopicInfo { + static const char* type_name() { + return "sensor_msgs::msg::dds_::PointField_"; + } + static const char* type_hash() { + return "RIHS01_5c6a4750728c2bcfbbf7037225b20b02d4429634732146b742dee1726637ef01"; + } + static size_t max_serialized_size() { return 272u; } +}; + +template<> struct CdrTopicInfo { + static const char* type_name() { + return "geometry_msgs::msg::dds_::Pose_"; + } + static const char* type_hash() { + return "RIHS01_d501954e9476cea2996984e812054b68026ae0bfae789d9a10b23daf35cc90fa"; + } + static size_t max_serialized_size() { return 56u; } +}; + +template<> struct CdrTopicInfo { + static const char* type_name() { + return "geometry_msgs::msg::dds_::PoseWithCovariance_"; + } + static const char* type_hash() { + return "RIHS01_9a7c0fd234b7f45c6098745ecccd773ca1085670e64107135397aee31c02e1bb"; + } + static size_t max_serialized_size() { return 344u; } +}; + +template<> struct CdrTopicInfo { + static const char* type_name() { + return "geometry_msgs::msg::dds_::Quaternion_"; + } + static const char* type_hash() { + return "RIHS01_8a765f66778c8ff7c8ab94afcc590a2ed5325a1d9a076ffff38fbce36f458684"; + } + static size_t max_serialized_size() { return 32u; } +}; + +template<> struct CdrTopicInfo { + static const char* type_name() { + return "sensor_msgs::msg::dds_::RegionOfInterest_"; + } + static const char* type_hash() { + return "RIHS01_ad16bcba5f9131dcdba6fbded19f726f5440e3c513b4fb586dd3027eeed8abb1"; + } + static size_t max_serialized_size() { return 17u; } +}; + +template<> struct CdrTopicInfo { + static const char* type_name() { + return "std_msgs::msg::dds_::String_"; + } + static const char* type_hash() { + return "RIHS01_df668c740482bbd48fb39d76a70dfd4bd59db1288021743503259e948f6b1a18"; + } + static size_t max_serialized_size() { return 260u; } +}; + +template<> struct CdrTopicInfo { + static const char* type_name() { + return "tf2_msgs::msg::dds_::TF2Error_"; + } + static const char* type_hash() { + return "RIHS01_db2485e7d6a0ec75bf087e058ee45df682acc3a621ca7780503d10aac141809a"; + } + static size_t max_serialized_size() { return 264u; } +}; + +template<> struct CdrTopicInfo { + static const char* type_name() { + return "tf2_msgs::msg::dds_::TFMessage_"; + } + static const char* type_hash() { + return "RIHS01_e369d0f05a23ae52508854b66f6aa0437f3449d652e8cbf22d5abe85d020f087"; + } + static size_t max_serialized_size() { return 58408u; } +}; + +template<> struct CdrTopicInfo { + static const char* type_name() { + return "builtin_interfaces::msg::dds_::Time_"; + } + static const char* type_hash() { + return "RIHS01_b106235e25a4c5ed35098aa0a61a3ee9c9b18d197f398b0e4206cea9acf9c197"; + } + static size_t max_serialized_size() { return 8u; } +}; + +template<> struct CdrTopicInfo { + static const char* type_name() { + return "geometry_msgs::msg::dds_::Transform_"; + } + static const char* type_hash() { + return "RIHS01_beb83fbe698636351461f6f35d1abb20010c43d55374d81bd041f1ba2581fddc"; + } + static size_t max_serialized_size() { return 56u; } +}; + +template<> struct CdrTopicInfo { + static const char* type_name() { + return "geometry_msgs::msg::dds_::TransformStamped_"; + } + static const char* type_hash() { + return "RIHS01_0a241f87d04668d94099cbb5ba11691d5ad32c2f29682e4eb5653424bd275206"; + } + static size_t max_serialized_size() { return 584u; } +}; + +template<> struct CdrTopicInfo { + static const char* type_name() { + return "geometry_msgs::msg::dds_::Twist_"; + } + static const char* type_hash() { + return "RIHS01_9c45bf16fe0983d80e3cfe750d6835843d265a9a6c46bd2e609fcddde6fb8d2a"; + } + static size_t max_serialized_size() { return 48u; } +}; + +template<> struct CdrTopicInfo { + static const char* type_name() { + return "geometry_msgs::msg::dds_::TwistWithCovariance_"; + } + static const char* type_hash() { + return "RIHS01_49f574f033f095d8b6cd1beaca5ca7925e296e84af1716d16c89d38b059c8c18"; + } + static size_t max_serialized_size() { return 336u; } +}; + +template<> struct CdrTopicInfo { + static const char* type_name() { + return "geometry_msgs::msg::dds_::Vector3_"; + } + static const char* type_hash() { + return "RIHS01_cc12fe83e4c02719f1ce8070bfd14aecd40f75a96696a67a2a1f37f7dbb0765d"; + } + static size_t max_serialized_size() { return 24u; } +}; + +} // namespace ros2 +} // namespace carla diff --git a/LibCarla/source/carla/ros2/types/UserDataFormat.h b/LibCarla/source/carla/ros2/types/UserDataFormat.h new file mode 100644 index 00000000000..2358e37e065 --- /dev/null +++ b/LibCarla/source/carla/ros2/types/UserDataFormat.h @@ -0,0 +1,64 @@ +// 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 . + +#pragma once + +#include "carla/ros2/types/CdrTopicInfo.h" + +#include +#include +#include + +namespace carla { +namespace ros2 { + +/// Builds the REP-2016 endpoint USER_DATA payload encoding a REP-2011 type +/// hash. +/// +/// Per OMG DDSI-RTPS v2.5 §9.6.2.2.2, PID_USER_DATA (0x002c) carries the +/// DDS UserDataQosPolicy octet sequence. ROS 2 REP-2016 defines the payload +/// as a set of ASCII key=value pairs separated by semicolons, e.g.: +/// +/// typehash=RIHS01_<64 lowercase hex>; +/// +/// rmw_cyclonedds_cpp and rmw_fastrtps_cpp on Iron / Jazzy parse this string +/// from PID_USER_DATA during SEDP endpoint discovery to perform REP-2011 +/// type-hash-based matching. When it is absent (null), Jazzy rmws emit: +/// [WARN] Failed to parse type hash ... from USER_DATA '(null)'. +/// +/// This helper is vendor-agnostic; the resulting byte vector is fed to: +/// FastDDS : DataWriterQos::user_data().data_vec() +/// DataReaderQos::user_data().data_vec() +/// CycloneDDS: dds_qset_userdata(qos, data, size) +/// +/// If type_hash is nullptr (used for types whose hash is unknown), an empty +/// vector is returned and user_data should NOT be set. +inline std::vector build_user_data(const char* type_hash) { + if (type_hash == nullptr) { + return {}; + } + // Payload: "typehash=;" (no null terminator — DDS carries length) + static const char prefix[] = "typehash="; + static const char suffix[] = ";"; + const size_t hash_len = std::strlen(type_hash); + const size_t total_len = sizeof(prefix) - 1u + hash_len + sizeof(suffix) - 1u; + + std::vector payload; + payload.reserve(total_len); + for (const char* p = prefix; *p; ++p) payload.push_back(static_cast(*p)); + for (size_t i = 0u; i < hash_len; ++i) payload.push_back(static_cast(type_hash[i])); + for (const char* p = suffix; *p; ++p) payload.push_back(static_cast(*p)); + return payload; +} + +/// Convenience: build user_data from the CdrTopicInfo::type_hash() of T. +/// Returns an empty vector (and the caller must skip setting user_data) when +/// CdrTopicInfo::type_hash() returns nullptr. +template +inline std::vector build_user_data_for() { + return build_user_data(CdrTopicInfo::type_hash()); +} + +} // namespace ros2 +} // namespace carla diff --git a/LibCarla/source/carla/ros2/types/msg/AckermannDrive.h b/LibCarla/source/carla/ros2/types/msg/AckermannDrive.h new file mode 100644 index 00000000000..259021b6f96 --- /dev/null +++ b/LibCarla/source/carla/ros2/types/msg/AckermannDrive.h @@ -0,0 +1,21 @@ +// 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 . + +#pragma once + +namespace carla { +namespace ros2 { +namespace msg { + +struct AckermannDrive { + float steering_angle = 0.0f; + float steering_angle_velocity = 0.0f; + float speed = 0.0f; + float acceleration = 0.0f; + float jerk = 0.0f; +}; + +} // namespace msg +} // namespace ros2 +} // namespace carla diff --git a/LibCarla/source/carla/ros2/types/msg/AckermannDriveStamped.h b/LibCarla/source/carla/ros2/types/msg/AckermannDriveStamped.h new file mode 100644 index 00000000000..003fe6823c1 --- /dev/null +++ b/LibCarla/source/carla/ros2/types/msg/AckermannDriveStamped.h @@ -0,0 +1,20 @@ +// 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 . + +#pragma once +#include "carla/ros2/types/msg/Header.h" +#include "carla/ros2/types/msg/AckermannDrive.h" + +namespace carla { +namespace ros2 { +namespace msg { + +struct AckermannDriveStamped { + Header header; + AckermannDrive drive; +}; + +} // namespace msg +} // namespace ros2 +} // namespace carla diff --git a/LibCarla/source/carla/ros2/types/msg/CameraInfo.h b/LibCarla/source/carla/ros2/types/msg/CameraInfo.h new file mode 100644 index 00000000000..1bc8fbf3a73 --- /dev/null +++ b/LibCarla/source/carla/ros2/types/msg/CameraInfo.h @@ -0,0 +1,33 @@ +// 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 . + +#pragma once +#include +#include +#include +#include +#include "carla/ros2/types/msg/Header.h" +#include "carla/ros2/types/msg/RegionOfInterest.h" + +namespace carla { +namespace ros2 { +namespace msg { + +struct CameraInfo { + Header header; + uint32_t height = 0; + uint32_t width = 0; + std::string distortion_model; + std::vector d; + std::array k = {}; + std::array r = {}; + std::array p = {}; + uint32_t binning_x = 0; + uint32_t binning_y = 0; + RegionOfInterest roi; +}; + +} // namespace msg +} // namespace ros2 +} // namespace carla diff --git a/LibCarla/source/carla/ros2/types/msg/CarlaCollisionEvent.h b/LibCarla/source/carla/ros2/types/msg/CarlaCollisionEvent.h new file mode 100644 index 00000000000..791cc9f90b9 --- /dev/null +++ b/LibCarla/source/carla/ros2/types/msg/CarlaCollisionEvent.h @@ -0,0 +1,22 @@ +// 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 . + +#pragma once +#include +#include "carla/ros2/types/msg/Header.h" +#include "carla/ros2/types/msg/Vector3.h" + +namespace carla { +namespace ros2 { +namespace msg { + +struct CarlaCollisionEvent { + Header header; + uint32_t other_actor_id = 0; + Vector3 normal_impulse; +}; + +} // namespace msg +} // namespace ros2 +} // namespace carla diff --git a/LibCarla/source/carla/ros2/types/msg/CarlaEgoVehicleControl.h b/LibCarla/source/carla/ros2/types/msg/CarlaEgoVehicleControl.h new file mode 100644 index 00000000000..9c7b90c4b96 --- /dev/null +++ b/LibCarla/source/carla/ros2/types/msg/CarlaEgoVehicleControl.h @@ -0,0 +1,26 @@ +// 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 . + +#pragma once +#include +#include "carla/ros2/types/msg/Header.h" + +namespace carla { +namespace ros2 { +namespace msg { + +struct CarlaEgoVehicleControl { + Header header; + float throttle = 0.0f; + float steer = 0.0f; + float brake = 0.0f; + bool hand_brake = false; + bool reverse = false; + int32_t gear = 0; + bool manual_gear_shift = false; +}; + +} // namespace msg +} // namespace ros2 +} // namespace carla diff --git a/LibCarla/source/carla/ros2/types/msg/CarlaLineInvasion.h b/LibCarla/source/carla/ros2/types/msg/CarlaLineInvasion.h new file mode 100644 index 00000000000..758ace94505 --- /dev/null +++ b/LibCarla/source/carla/ros2/types/msg/CarlaLineInvasion.h @@ -0,0 +1,21 @@ +// 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 . + +#pragma once +#include +#include +#include "carla/ros2/types/msg/Header.h" + +namespace carla { +namespace ros2 { +namespace msg { + +struct CarlaLineInvasion { + Header header; + std::vector crossed_lane_markings; +}; + +} // namespace msg +} // namespace ros2 +} // namespace carla diff --git a/LibCarla/source/carla/ros2/types/msg/Clock.h b/LibCarla/source/carla/ros2/types/msg/Clock.h new file mode 100644 index 00000000000..c3aeb22ef23 --- /dev/null +++ b/LibCarla/source/carla/ros2/types/msg/Clock.h @@ -0,0 +1,18 @@ +// 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 . + +#pragma once +#include "carla/ros2/types/msg/Time.h" + +namespace carla { +namespace ros2 { +namespace msg { + +struct Clock { + Time clock; +}; + +} // namespace msg +} // namespace ros2 +} // namespace carla diff --git a/LibCarla/source/carla/ros2/types/msg/Float32.h b/LibCarla/source/carla/ros2/types/msg/Float32.h new file mode 100644 index 00000000000..6452f152256 --- /dev/null +++ b/LibCarla/source/carla/ros2/types/msg/Float32.h @@ -0,0 +1,17 @@ +// 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 . + +#pragma once + +namespace carla { +namespace ros2 { +namespace msg { + +struct Float32 { + float data = 0.0f; +}; + +} // namespace msg +} // namespace ros2 +} // namespace carla diff --git a/LibCarla/source/carla/ros2/types/msg/Header.h b/LibCarla/source/carla/ros2/types/msg/Header.h new file mode 100644 index 00000000000..7def89c2f96 --- /dev/null +++ b/LibCarla/source/carla/ros2/types/msg/Header.h @@ -0,0 +1,20 @@ +// 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 . + +#pragma once +#include +#include "carla/ros2/types/msg/Time.h" + +namespace carla { +namespace ros2 { +namespace msg { + +struct Header { + Time stamp; + std::string frame_id; +}; + +} // namespace msg +} // namespace ros2 +} // namespace carla diff --git a/LibCarla/source/carla/ros2/types/msg/Image.h b/LibCarla/source/carla/ros2/types/msg/Image.h new file mode 100644 index 00000000000..6f106c1735e --- /dev/null +++ b/LibCarla/source/carla/ros2/types/msg/Image.h @@ -0,0 +1,27 @@ +// 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 . + +#pragma once +#include +#include +#include +#include "carla/ros2/types/msg/Header.h" + +namespace carla { +namespace ros2 { +namespace msg { + +struct Image { + Header header; + uint32_t height = 0; + uint32_t width = 0; + std::string encoding; + uint8_t is_bigendian = 0; + uint32_t step = 0; + std::vector data; +}; + +} // namespace msg +} // namespace ros2 +} // namespace carla diff --git a/LibCarla/source/carla/ros2/types/msg/Imu.h b/LibCarla/source/carla/ros2/types/msg/Imu.h new file mode 100644 index 00000000000..357c4e8fe07 --- /dev/null +++ b/LibCarla/source/carla/ros2/types/msg/Imu.h @@ -0,0 +1,27 @@ +// 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 . + +#pragma once +#include +#include "carla/ros2/types/msg/Header.h" +#include "carla/ros2/types/msg/Quaternion.h" +#include "carla/ros2/types/msg/Vector3.h" + +namespace carla { +namespace ros2 { +namespace msg { + +struct Imu { + Header header; + Quaternion orientation; + std::array orientation_covariance = {}; + Vector3 angular_velocity; + std::array angular_velocity_covariance = {}; + Vector3 linear_acceleration; + std::array linear_acceleration_covariance = {}; +}; + +} // namespace msg +} // namespace ros2 +} // namespace carla diff --git a/LibCarla/source/carla/ros2/types/msg/NavSatFix.h b/LibCarla/source/carla/ros2/types/msg/NavSatFix.h new file mode 100644 index 00000000000..842b777faef --- /dev/null +++ b/LibCarla/source/carla/ros2/types/msg/NavSatFix.h @@ -0,0 +1,32 @@ +// 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 . + +#pragma once +#include +#include +#include "carla/ros2/types/msg/Header.h" +#include "carla/ros2/types/msg/NavSatStatus.h" + +namespace carla { +namespace ros2 { +namespace msg { + +struct NavSatFix { + static const uint8_t COVARIANCE_TYPE_UNKNOWN = 0; + static const uint8_t COVARIANCE_TYPE_APPROXIMATED = 1; + static const uint8_t COVARIANCE_TYPE_DIAGONAL_KNOWN = 2; + static const uint8_t COVARIANCE_TYPE_KNOWN = 3; + + Header header; + NavSatStatus status; + double latitude = 0.0; + double longitude = 0.0; + double altitude = 0.0; + std::array position_covariance = {}; + uint8_t position_covariance_type = 0; +}; + +} // namespace msg +} // namespace ros2 +} // namespace carla diff --git a/LibCarla/source/carla/ros2/types/msg/NavSatStatus.h b/LibCarla/source/carla/ros2/types/msg/NavSatStatus.h new file mode 100644 index 00000000000..2470b2e9968 --- /dev/null +++ b/LibCarla/source/carla/ros2/types/msg/NavSatStatus.h @@ -0,0 +1,28 @@ +// 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 . + +#pragma once +#include + +namespace carla { +namespace ros2 { +namespace msg { + +struct NavSatStatus { + static const uint8_t STATUS_NO_FIX = 255; + static const uint8_t STATUS_FIX = 0; + static const uint8_t STATUS_SBAS_FIX = 1; + static const uint8_t STATUS_GBAS_FIX = 2; + static const uint16_t SERVICE_GPS = 1; + static const uint16_t SERVICE_GLONASS = 2; + static const uint16_t SERVICE_COMPASS = 4; + static const uint16_t SERVICE_GALILEO = 8; + + uint8_t status = 0; + uint16_t service = 0; +}; + +} // namespace msg +} // namespace ros2 +} // namespace carla diff --git a/LibCarla/source/carla/ros2/types/msg/Odometry.h b/LibCarla/source/carla/ros2/types/msg/Odometry.h new file mode 100644 index 00000000000..4d4e2d0a3e8 --- /dev/null +++ b/LibCarla/source/carla/ros2/types/msg/Odometry.h @@ -0,0 +1,24 @@ +// 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 . + +#pragma once +#include +#include "carla/ros2/types/msg/Header.h" +#include "carla/ros2/types/msg/PoseWithCovariance.h" +#include "carla/ros2/types/msg/TwistWithCovariance.h" + +namespace carla { +namespace ros2 { +namespace msg { + +struct Odometry { + Header header; + std::string child_frame_id; + PoseWithCovariance pose; + TwistWithCovariance twist; +}; + +} // namespace msg +} // namespace ros2 +} // namespace carla diff --git a/LibCarla/source/carla/ros2/types/msg/Point.h b/LibCarla/source/carla/ros2/types/msg/Point.h new file mode 100644 index 00000000000..b731e4a19df --- /dev/null +++ b/LibCarla/source/carla/ros2/types/msg/Point.h @@ -0,0 +1,19 @@ +// 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 . + +#pragma once + +namespace carla { +namespace ros2 { +namespace msg { + +struct Point { + double x = 0.0; + double y = 0.0; + double z = 0.0; +}; + +} // namespace msg +} // namespace ros2 +} // namespace carla diff --git a/LibCarla/source/carla/ros2/types/msg/Point32.h b/LibCarla/source/carla/ros2/types/msg/Point32.h new file mode 100644 index 00000000000..f3ee652542d --- /dev/null +++ b/LibCarla/source/carla/ros2/types/msg/Point32.h @@ -0,0 +1,19 @@ +// 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 . + +#pragma once + +namespace carla { +namespace ros2 { +namespace msg { + +struct Point32 { + float x = 0.0f; + float y = 0.0f; + float z = 0.0f; +}; + +} // namespace msg +} // namespace ros2 +} // namespace carla diff --git a/LibCarla/source/carla/ros2/types/msg/PointCloud2.h b/LibCarla/source/carla/ros2/types/msg/PointCloud2.h new file mode 100644 index 00000000000..4cd62edb0d7 --- /dev/null +++ b/LibCarla/source/carla/ros2/types/msg/PointCloud2.h @@ -0,0 +1,29 @@ +// 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 . + +#pragma once +#include +#include +#include "carla/ros2/types/msg/Header.h" +#include "carla/ros2/types/msg/PointField.h" + +namespace carla { +namespace ros2 { +namespace msg { + +struct PointCloud2 { + Header header; + uint32_t height = 0; + uint32_t width = 0; + std::vector fields; + bool is_bigendian = false; + uint32_t point_step = 0; + uint32_t row_step = 0; + std::vector data; + bool is_dense = false; +}; + +} // namespace msg +} // namespace ros2 +} // namespace carla diff --git a/LibCarla/source/carla/ros2/types/msg/PointField.h b/LibCarla/source/carla/ros2/types/msg/PointField.h new file mode 100644 index 00000000000..85d9f8da983 --- /dev/null +++ b/LibCarla/source/carla/ros2/types/msg/PointField.h @@ -0,0 +1,31 @@ +// 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 . + +#pragma once +#include +#include + +namespace carla { +namespace ros2 { +namespace msg { + +struct PointField { + static const uint8_t INT8 = 1; + static const uint8_t UINT8 = 2; + static const uint8_t INT16 = 3; + static const uint8_t UINT16 = 4; + static const uint8_t INT32 = 5; + static const uint8_t UINT32 = 6; + static const uint8_t FLOAT32 = 7; + static const uint8_t FLOAT64 = 8; + + std::string name; + uint32_t offset = 0; + uint8_t datatype = 0; + uint32_t count = 0; +}; + +} // namespace msg +} // namespace ros2 +} // namespace carla diff --git a/LibCarla/source/carla/ros2/types/msg/Pose.h b/LibCarla/source/carla/ros2/types/msg/Pose.h new file mode 100644 index 00000000000..bf3c81b869a --- /dev/null +++ b/LibCarla/source/carla/ros2/types/msg/Pose.h @@ -0,0 +1,20 @@ +// 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 . + +#pragma once +#include "carla/ros2/types/msg/Point.h" +#include "carla/ros2/types/msg/Quaternion.h" + +namespace carla { +namespace ros2 { +namespace msg { + +struct Pose { + Point position; + Quaternion orientation; +}; + +} // namespace msg +} // namespace ros2 +} // namespace carla diff --git a/LibCarla/source/carla/ros2/types/msg/PoseWithCovariance.h b/LibCarla/source/carla/ros2/types/msg/PoseWithCovariance.h new file mode 100644 index 00000000000..3570f8f0a50 --- /dev/null +++ b/LibCarla/source/carla/ros2/types/msg/PoseWithCovariance.h @@ -0,0 +1,20 @@ +// 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 . + +#pragma once +#include +#include "carla/ros2/types/msg/Pose.h" + +namespace carla { +namespace ros2 { +namespace msg { + +struct PoseWithCovariance { + Pose pose; + std::array covariance = {}; +}; + +} // namespace msg +} // namespace ros2 +} // namespace carla diff --git a/LibCarla/source/carla/ros2/types/msg/Quaternion.h b/LibCarla/source/carla/ros2/types/msg/Quaternion.h new file mode 100644 index 00000000000..59cecdf4725 --- /dev/null +++ b/LibCarla/source/carla/ros2/types/msg/Quaternion.h @@ -0,0 +1,20 @@ +// 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 . + +#pragma once + +namespace carla { +namespace ros2 { +namespace msg { + +struct Quaternion { + double x = 0.0; + double y = 0.0; + double z = 0.0; + double w = 0.0; +}; + +} // namespace msg +} // namespace ros2 +} // namespace carla diff --git a/LibCarla/source/carla/ros2/types/msg/RegionOfInterest.h b/LibCarla/source/carla/ros2/types/msg/RegionOfInterest.h new file mode 100644 index 00000000000..0a21608c655 --- /dev/null +++ b/LibCarla/source/carla/ros2/types/msg/RegionOfInterest.h @@ -0,0 +1,22 @@ +// 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 . + +#pragma once +#include + +namespace carla { +namespace ros2 { +namespace msg { + +struct RegionOfInterest { + uint32_t x_offset = 0; + uint32_t y_offset = 0; + uint32_t height = 0; + uint32_t width = 0; + bool do_rectify = false; +}; + +} // namespace msg +} // namespace ros2 +} // namespace carla diff --git a/LibCarla/source/carla/ros2/types/msg/String.h b/LibCarla/source/carla/ros2/types/msg/String.h new file mode 100644 index 00000000000..81a50e40248 --- /dev/null +++ b/LibCarla/source/carla/ros2/types/msg/String.h @@ -0,0 +1,18 @@ +// 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 . + +#pragma once +#include + +namespace carla { +namespace ros2 { +namespace msg { + +struct String { + std::string data; +}; + +} // namespace msg +} // namespace ros2 +} // namespace carla diff --git a/LibCarla/source/carla/ros2/types/msg/TF2Error.h b/LibCarla/source/carla/ros2/types/msg/TF2Error.h new file mode 100644 index 00000000000..33f4bf46e8f --- /dev/null +++ b/LibCarla/source/carla/ros2/types/msg/TF2Error.h @@ -0,0 +1,20 @@ +// 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 . + +#pragma once +#include +#include + +namespace carla { +namespace ros2 { +namespace msg { + +struct TF2Error { + uint8_t error = 0; + std::string error_string; +}; + +} // namespace msg +} // namespace ros2 +} // namespace carla diff --git a/LibCarla/source/carla/ros2/types/msg/TFMessage.h b/LibCarla/source/carla/ros2/types/msg/TFMessage.h new file mode 100644 index 00000000000..54636dffa79 --- /dev/null +++ b/LibCarla/source/carla/ros2/types/msg/TFMessage.h @@ -0,0 +1,19 @@ +// 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 . + +#pragma once +#include +#include "carla/ros2/types/msg/TransformStamped.h" + +namespace carla { +namespace ros2 { +namespace msg { + +struct TFMessage { + std::vector transforms; +}; + +} // namespace msg +} // namespace ros2 +} // namespace carla diff --git a/LibCarla/source/carla/ros2/types/msg/Time.h b/LibCarla/source/carla/ros2/types/msg/Time.h new file mode 100644 index 00000000000..4183173ee83 --- /dev/null +++ b/LibCarla/source/carla/ros2/types/msg/Time.h @@ -0,0 +1,19 @@ +// 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 . + +#pragma once +#include + +namespace carla { +namespace ros2 { +namespace msg { + +struct Time { + int32_t sec = 0; + uint32_t nanosec = 0; +}; + +} // namespace msg +} // namespace ros2 +} // namespace carla diff --git a/LibCarla/source/carla/ros2/types/msg/Transform.h b/LibCarla/source/carla/ros2/types/msg/Transform.h new file mode 100644 index 00000000000..99ec1d2b9c7 --- /dev/null +++ b/LibCarla/source/carla/ros2/types/msg/Transform.h @@ -0,0 +1,20 @@ +// 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 . + +#pragma once +#include "carla/ros2/types/msg/Vector3.h" +#include "carla/ros2/types/msg/Quaternion.h" + +namespace carla { +namespace ros2 { +namespace msg { + +struct Transform { + Vector3 translation; + Quaternion rotation; +}; + +} // namespace msg +} // namespace ros2 +} // namespace carla diff --git a/LibCarla/source/carla/ros2/types/msg/TransformStamped.h b/LibCarla/source/carla/ros2/types/msg/TransformStamped.h new file mode 100644 index 00000000000..cb9d471c3bd --- /dev/null +++ b/LibCarla/source/carla/ros2/types/msg/TransformStamped.h @@ -0,0 +1,22 @@ +// 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 . + +#pragma once +#include +#include "carla/ros2/types/msg/Header.h" +#include "carla/ros2/types/msg/Transform.h" + +namespace carla { +namespace ros2 { +namespace msg { + +struct TransformStamped { + Header header; + std::string child_frame_id; + Transform transform; +}; + +} // namespace msg +} // namespace ros2 +} // namespace carla diff --git a/LibCarla/source/carla/ros2/types/msg/Twist.h b/LibCarla/source/carla/ros2/types/msg/Twist.h new file mode 100644 index 00000000000..ed2d8d42e36 --- /dev/null +++ b/LibCarla/source/carla/ros2/types/msg/Twist.h @@ -0,0 +1,19 @@ +// 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 . + +#pragma once +#include "carla/ros2/types/msg/Vector3.h" + +namespace carla { +namespace ros2 { +namespace msg { + +struct Twist { + Vector3 linear; + Vector3 angular; +}; + +} // namespace msg +} // namespace ros2 +} // namespace carla diff --git a/LibCarla/source/carla/ros2/types/msg/TwistWithCovariance.h b/LibCarla/source/carla/ros2/types/msg/TwistWithCovariance.h new file mode 100644 index 00000000000..6e00857d6c1 --- /dev/null +++ b/LibCarla/source/carla/ros2/types/msg/TwistWithCovariance.h @@ -0,0 +1,20 @@ +// 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 . + +#pragma once +#include +#include "carla/ros2/types/msg/Twist.h" + +namespace carla { +namespace ros2 { +namespace msg { + +struct TwistWithCovariance { + Twist twist; + std::array covariance = {}; +}; + +} // namespace msg +} // namespace ros2 +} // namespace carla diff --git a/LibCarla/source/carla/ros2/types/msg/Vector3.h b/LibCarla/source/carla/ros2/types/msg/Vector3.h new file mode 100644 index 00000000000..1b879020e11 --- /dev/null +++ b/LibCarla/source/carla/ros2/types/msg/Vector3.h @@ -0,0 +1,19 @@ +// 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 . + +#pragma once + +namespace carla { +namespace ros2 { +namespace msg { + +struct Vector3 { + double x = 0.0; + double y = 0.0; + double z = 0.0; +}; + +} // namespace msg +} // namespace ros2 +} // namespace carla From 28421e7b75e6e220dda40a93e47f5b8b6aed4dc0 Mon Sep 17 00:00:00 2001 From: Jesus Armando Anaya Date: Mon, 6 Jul 2026 23:45:05 -0700 Subject: [PATCH 3/7] test(ros2): add CDR serialization round-trip tests Add LibCarla/source/test/server/test_ros2_serialization.cpp with the cdr_topic_info (2 cases) and cdr_serialization (22 cases) GTest groups from the ue4-dev middleware suite: POD round-trips for every message family, a golden-bytes guard that pins the classic CDR little-endian encapsulation so any drift to XCDRv2 or big-endian is caught deterministically, truncated / corrupt-encapsulation / hostile-sequence-length rejection paths exercising the kMaxCdrSequenceElements cap, and the contract that cdr_serialized_size(msg) equals serialize_to_cdr(msg).size(). The tests exercise only Fast-CDR plus the new foundation headers; no DDS participant is created and libfastrtps is not linked. Build wiring, gated on ENABLE_ROS2: - libcarla_test_server gains the Ros2Native install include dir as a SYSTEM include and links libfastcdr.so from the fastdds ExternalProject install tree; add_dependencies orders the test compile and link after the fastdds install step. - The fastdds ExternalProject declares libfastcdr.so via INSTALL_BYPRODUCTS so Ninja accepts the full-path link input on a clean tree. - Without ENABLE_ROS2 the test source is removed from the server test glob and the suite builds exactly as before. (adapted from ue4-dev f53144c97f5f1225606b9d4f8be4e35af1c7d5f8) (adapted from ue4-dev b8650880de0c0c9ec9e9dffc149fdae33094ca02) --- LibCarla/CMakeLists.txt | 24 + .../test/server/test_ros2_serialization.cpp | 603 ++++++++++++++++++ Ros2Native/CMakeLists.txt | 2 + 3 files changed, 629 insertions(+) create mode 100644 LibCarla/source/test/server/test_ros2_serialization.cpp diff --git a/LibCarla/CMakeLists.txt b/LibCarla/CMakeLists.txt index e67473d534c..dd38abbd44a 100644 --- a/LibCarla/CMakeLists.txt +++ b/LibCarla/CMakeLists.txt @@ -463,6 +463,14 @@ if (BUILD_LIBCARLA_TESTS) ${LIBCARLA_SOURCE_PATH}/test/server/*.cpp ) + if (NOT ENABLE_ROS2) + list ( + REMOVE_ITEM + LIBCARLA_TEST_SERVER_SOURCES + ${LIBCARLA_SOURCE_PATH}/test/server/test_ros2_serialization.cpp + ) + endif () + carla_add_executable ( libcarla_test_server "Build the LibCarla server-side gtest suite." @@ -491,6 +499,22 @@ if (BUILD_LIBCARLA_TESTS) LIBCARLA_TEST_CONTENT_FOLDER="${CARLA_WORKSPACE_PATH}/Build/test-content" ) + if (ENABLE_ROS2) + # The CDR serialization tests need the Fast-CDR headers and shared lib + # that the Ros2Native ExternalProject chain installs at build time. The + # path mirrors PROJECT_INSTALL_PATH in Ros2Native/CMakeLists.txt. + set (CARLA_ROS2_NATIVE_INSTALL_PATH ${CMAKE_BINARY_DIR}/Ros2Native/install) + target_include_directories ( + libcarla_test_server SYSTEM PRIVATE + ${CARLA_ROS2_NATIVE_INSTALL_PATH}/include + ) + target_link_libraries ( + libcarla_test_server PRIVATE + ${CARLA_ROS2_NATIVE_INSTALL_PATH}/lib/libfastcdr.so + ) + add_dependencies (libcarla_test_server fastdds) + endif () + endif () if (BUILD_CARLA_CLIENT) diff --git a/LibCarla/source/test/server/test_ros2_serialization.cpp b/LibCarla/source/test/server/test_ros2_serialization.cpp new file mode 100644 index 00000000000..243159e903c --- /dev/null +++ b/LibCarla/source/test/server/test_ros2_serialization.cpp @@ -0,0 +1,603 @@ +// 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 . + +// Must be defined before any includes to suppress real DDS auto-includes +// when the middleware abstraction test groups join this suite. +#define CARLA_ROS2_MIDDLEWARE_TESTING + +#include "test.h" + +#include +#include + +#include +#include + +// ========================================================================== +// Group 1: cdr_topic_info (2 tests) +// ========================================================================== + +TEST(cdr_topic_info, type_names_are_non_empty) { + EXPECT_STRNE("", carla::ros2::CdrTopicInfo::type_name()); + EXPECT_STRNE("", carla::ros2::CdrTopicInfo::type_name()); + EXPECT_STRNE("", carla::ros2::CdrTopicInfo::type_name()); + EXPECT_STRNE("", carla::ros2::CdrTopicInfo::type_name()); + EXPECT_STRNE("", carla::ros2::CdrTopicInfo::type_name()); + EXPECT_STRNE("", carla::ros2::CdrTopicInfo::type_name()); + EXPECT_STRNE("", carla::ros2::CdrTopicInfo::type_name()); + EXPECT_STRNE("", carla::ros2::CdrTopicInfo::type_name()); + EXPECT_STRNE("", carla::ros2::CdrTopicInfo::type_name()); + EXPECT_STRNE("", carla::ros2::CdrTopicInfo::type_name()); + EXPECT_STRNE("", carla::ros2::CdrTopicInfo::type_name()); + EXPECT_STRNE("", carla::ros2::CdrTopicInfo::type_name()); + EXPECT_STRNE("", carla::ros2::CdrTopicInfo::type_name()); + EXPECT_STRNE("", carla::ros2::CdrTopicInfo::type_name()); + EXPECT_STRNE("", carla::ros2::CdrTopicInfo::type_name()); +} + +TEST(cdr_topic_info, max_sizes_are_positive) { + EXPECT_GT(carla::ros2::CdrTopicInfo::max_serialized_size(), 0u); + EXPECT_GT(carla::ros2::CdrTopicInfo::max_serialized_size(), 0u); + EXPECT_GT(carla::ros2::CdrTopicInfo::max_serialized_size(), 0u); + EXPECT_GT(carla::ros2::CdrTopicInfo::max_serialized_size(), 0u); + EXPECT_GT(carla::ros2::CdrTopicInfo::max_serialized_size(), 0u); + EXPECT_GT(carla::ros2::CdrTopicInfo::max_serialized_size(), 0u); + EXPECT_GT(carla::ros2::CdrTopicInfo::max_serialized_size(), 0u); + EXPECT_GT(carla::ros2::CdrTopicInfo::max_serialized_size(), 0u); + EXPECT_GT(carla::ros2::CdrTopicInfo::max_serialized_size(), 0u); + EXPECT_GT(carla::ros2::CdrTopicInfo::max_serialized_size(), 0u); +} + +// ========================================================================== +// Group 2: cdr_serialization (22 tests) +// ========================================================================== + +TEST(cdr_serialization, time_round_trip) { + carla::ros2::msg::Time original{}; + original.sec = 42; + original.nanosec = 123456789u; + + auto buf = carla::ros2::serialize_to_cdr(original); + ASSERT_FALSE(buf.empty()); + + carla::ros2::msg::Time recovered{}; + EXPECT_TRUE(carla::ros2::deserialize_from_cdr(buf.data(), buf.size(), recovered)); + EXPECT_EQ(recovered.sec, 42); + EXPECT_EQ(recovered.nanosec, 123456789u); +} + +// Byte-exact wire-format guard. The symmetric round-trip tests above cannot +// detect an encoding drift (e.g. Fast-CDR defaulting to XCDRv2, which inserts +// DHEADERs) because deserialize_from_cdr would consume whatever encoding +// serialize_to_cdr emitted. This test pins the literal bytes so a regression +// to XCDRv2 or big-endian is caught deterministically. These exact bytes are +// what every backend puts on the wire: FastDDS via write_serialized_payload, +// CycloneDDS via dds_writecdr (raw passthrough of serialize_to_cdr), and Zenoh. +// Layout (classic CDR, encoding version 1, little-endian): +// 00 01 00 00 encapsulation header (PLAIN_CDR little-endian + 2 option bytes) +// 2A 00 00 00 sec = int32 42 (0x0000002A, LE) +// 15 CD 5B 07 nanosec= uint32 123456789 (0x075BCD15, LE) +TEST(cdr_serialization, time_golden_xcdrv1_bytes) { + carla::ros2::msg::Time original{}; + original.sec = 42; + original.nanosec = 123456789u; + + const auto buf = carla::ros2::serialize_to_cdr(original); + + const std::vector expected{ + 0x00u, 0x01u, 0x00u, 0x00u, + 0x2Au, 0x00u, 0x00u, 0x00u, + 0x15u, 0xCDu, 0x5Bu, 0x07u}; + // Assert the size first: a failed or short serialization then yields a + // targeted size mismatch instead of a noisy full-vector diff. + ASSERT_EQ(buf.size(), expected.size()); + EXPECT_EQ(buf, expected); + // Discriminator byte: classic CDR_LE is 0x01; any XCDRv2 representation + // (PLAIN_CDR2 / DELIMIT_CDR2 / PL_CDR2) would change byte 1. + EXPECT_EQ(buf[0], 0x00u); + EXPECT_EQ(buf[1], 0x01u); +} + +TEST(cdr_serialization, header_round_trip) { + carla::ros2::msg::Header original{}; + original.stamp.sec = 10; + original.stamp.nanosec = 500000000u; + original.frame_id = "base_link"; + + auto buf = carla::ros2::serialize_to_cdr(original); + ASSERT_FALSE(buf.empty()); + + carla::ros2::msg::Header recovered{}; + EXPECT_TRUE(carla::ros2::deserialize_from_cdr(buf.data(), buf.size(), recovered)); + EXPECT_EQ(recovered.stamp.sec, 10); + EXPECT_EQ(recovered.stamp.nanosec, 500000000u); + EXPECT_EQ(recovered.frame_id, "base_link"); +} + +TEST(cdr_serialization, vector3_round_trip) { + carla::ros2::msg::Vector3 original{}; + original.x = 1.5; + original.y = -2.75; + original.z = 3.0; + + auto buf = carla::ros2::serialize_to_cdr(original); + ASSERT_FALSE(buf.empty()); + + carla::ros2::msg::Vector3 recovered{}; + EXPECT_TRUE(carla::ros2::deserialize_from_cdr(buf.data(), buf.size(), recovered)); + EXPECT_DOUBLE_EQ(recovered.x, 1.5); + EXPECT_DOUBLE_EQ(recovered.y, -2.75); + EXPECT_DOUBLE_EQ(recovered.z, 3.0); +} + +TEST(cdr_serialization, imu_round_trip) { + carla::ros2::msg::Imu original{}; + original.header.stamp.sec = 5; + original.header.frame_id = "imu_link"; + original.orientation.x = 0.1; + original.orientation.y = 0.2; + original.orientation.z = 0.3; + original.orientation.w = 0.9; + original.angular_velocity.x = 0.01; + original.linear_acceleration.z = 9.81; + original.orientation_covariance[0] = 1.0; + original.orientation_covariance[4] = 1.0; + original.orientation_covariance[8] = 1.0; + + auto buf = carla::ros2::serialize_to_cdr(original); + ASSERT_FALSE(buf.empty()); + + carla::ros2::msg::Imu recovered{}; + EXPECT_TRUE(carla::ros2::deserialize_from_cdr(buf.data(), buf.size(), recovered)); + EXPECT_EQ(recovered.header.stamp.sec, 5); + EXPECT_EQ(recovered.header.frame_id, "imu_link"); + EXPECT_DOUBLE_EQ(recovered.orientation.x, 0.1); + EXPECT_DOUBLE_EQ(recovered.orientation.w, 0.9); + EXPECT_DOUBLE_EQ(recovered.linear_acceleration.z, 9.81); + EXPECT_DOUBLE_EQ(recovered.orientation_covariance[0], 1.0); + EXPECT_DOUBLE_EQ(recovered.orientation_covariance[4], 1.0); +} + +TEST(cdr_serialization, image_round_trip) { + carla::ros2::msg::Image original{}; + original.header.frame_id = "camera"; + original.height = 2u; + original.width = 3u; + original.encoding = "rgb8"; + original.is_bigendian = 0u; + original.step = 9u; + original.data = {1u, 2u, 3u, 4u, 5u, 6u}; + + auto buf = carla::ros2::serialize_to_cdr(original); + ASSERT_FALSE(buf.empty()); + + carla::ros2::msg::Image recovered{}; + EXPECT_TRUE(carla::ros2::deserialize_from_cdr(buf.data(), buf.size(), recovered)); + EXPECT_EQ(recovered.header.frame_id, "camera"); + EXPECT_EQ(recovered.height, 2u); + EXPECT_EQ(recovered.width, 3u); + EXPECT_EQ(recovered.encoding, "rgb8"); + ASSERT_EQ(recovered.data.size(), 6u); + EXPECT_EQ(recovered.data[0], 1u); + EXPECT_EQ(recovered.data[5], 6u); +} + +TEST(cdr_serialization, pointcloud2_round_trip) { + carla::ros2::msg::PointCloud2 original{}; + original.header.frame_id = "velodyne"; + original.height = 1u; + original.width = 2u; + + carla::ros2::msg::PointField pf{}; + pf.name = "x"; + pf.offset = 0u; + pf.datatype = static_cast(carla::ros2::msg::PointField::FLOAT32); + pf.count = 1u; + original.fields.push_back(pf); + + original.is_bigendian = false; + original.point_step = 4u; + original.row_step = 8u; + original.data = {0u, 0u, 128u, 63u, // 1.0f LE + 0u, 0u, 0u, 64u}; // 2.0f LE + original.is_dense = true; + + auto buf = carla::ros2::serialize_to_cdr(original); + ASSERT_FALSE(buf.empty()); + + carla::ros2::msg::PointCloud2 recovered{}; + EXPECT_TRUE(carla::ros2::deserialize_from_cdr(buf.data(), buf.size(), recovered)); + EXPECT_EQ(recovered.header.frame_id, "velodyne"); + EXPECT_EQ(recovered.height, 1u); + EXPECT_EQ(recovered.width, 2u); + ASSERT_EQ(recovered.fields.size(), 1u); + EXPECT_EQ(recovered.fields[0].name, "x"); + EXPECT_EQ(recovered.fields[0].datatype, static_cast(carla::ros2::msg::PointField::FLOAT32)); + EXPECT_EQ(recovered.is_dense, true); + ASSERT_EQ(recovered.data.size(), 8u); +} + +TEST(cdr_serialization, tfmessage_round_trip) { + carla::ros2::msg::TFMessage original{}; + + carla::ros2::msg::TransformStamped ts{}; + ts.header.stamp.sec = 1; + ts.header.frame_id = "world"; + ts.child_frame_id = "robot"; + ts.transform.translation.x = 1.0; + ts.transform.translation.y = 2.0; + ts.transform.translation.z = 0.5; + ts.transform.rotation.w = 1.0; + original.transforms.push_back(ts); + + auto buf = carla::ros2::serialize_to_cdr(original); + ASSERT_FALSE(buf.empty()); + + carla::ros2::msg::TFMessage recovered{}; + EXPECT_TRUE(carla::ros2::deserialize_from_cdr(buf.data(), buf.size(), recovered)); + ASSERT_EQ(recovered.transforms.size(), 1u); + EXPECT_EQ(recovered.transforms[0].header.stamp.sec, 1); + EXPECT_EQ(recovered.transforms[0].header.frame_id, "world"); + EXPECT_EQ(recovered.transforms[0].child_frame_id, "robot"); + EXPECT_DOUBLE_EQ(recovered.transforms[0].transform.translation.x, 1.0); + EXPECT_DOUBLE_EQ(recovered.transforms[0].transform.translation.y, 2.0); + EXPECT_DOUBLE_EQ(recovered.transforms[0].transform.rotation.w, 1.0); +} + +TEST(cdr_serialization, navsat_fix_round_trip) { + carla::ros2::msg::NavSatFix original{}; + original.header.frame_id = "gps"; + original.status.status = static_cast(carla::ros2::msg::NavSatStatus::STATUS_FIX); + original.status.service = static_cast(carla::ros2::msg::NavSatStatus::SERVICE_GPS); + original.latitude = 48.8566; + original.longitude = 2.3522; + original.altitude = 35.0; + original.position_covariance[0] = 0.01; + original.position_covariance_type = + static_cast(carla::ros2::msg::NavSatFix::COVARIANCE_TYPE_DIAGONAL_KNOWN); + + auto buf = carla::ros2::serialize_to_cdr(original); + ASSERT_FALSE(buf.empty()); + + carla::ros2::msg::NavSatFix recovered{}; + EXPECT_TRUE(carla::ros2::deserialize_from_cdr(buf.data(), buf.size(), recovered)); + EXPECT_EQ(recovered.header.frame_id, "gps"); + EXPECT_EQ( + recovered.status.status, + static_cast(carla::ros2::msg::NavSatStatus::STATUS_FIX)); + EXPECT_DOUBLE_EQ(recovered.latitude, 48.8566); + EXPECT_DOUBLE_EQ(recovered.longitude, 2.3522); + EXPECT_DOUBLE_EQ(recovered.altitude, 35.0); + EXPECT_DOUBLE_EQ(recovered.position_covariance[0], 0.01); + EXPECT_EQ( + recovered.position_covariance_type, + static_cast(carla::ros2::msg::NavSatFix::COVARIANCE_TYPE_DIAGONAL_KNOWN)); +} + +TEST(cdr_serialization, carla_ego_vehicle_control_round_trip) { + carla::ros2::msg::CarlaEgoVehicleControl original{}; + original.throttle = 0.75f; + original.steer = -0.5f; + original.brake = 0.0f; + original.hand_brake = false; + original.reverse = true; + original.gear = 2; + original.manual_gear_shift = false; + + auto buf = carla::ros2::serialize_to_cdr(original); + ASSERT_FALSE(buf.empty()); + + carla::ros2::msg::CarlaEgoVehicleControl recovered{}; + EXPECT_TRUE(carla::ros2::deserialize_from_cdr(buf.data(), buf.size(), recovered)); + EXPECT_FLOAT_EQ(recovered.throttle, 0.75f); + EXPECT_FLOAT_EQ(recovered.steer, -0.5f); + EXPECT_FLOAT_EQ(recovered.brake, 0.0f); + EXPECT_EQ(recovered.hand_brake, false); + EXPECT_EQ(recovered.reverse, true); + EXPECT_EQ(recovered.gear, 2); + EXPECT_EQ(recovered.manual_gear_shift, false); +} + +TEST(cdr_serialization, carla_line_invasion_round_trip) { + carla::ros2::msg::CarlaLineInvasion original{}; + original.header.frame_id = "vehicle"; + original.crossed_lane_markings = {1, 4, 7}; + + auto buf = carla::ros2::serialize_to_cdr(original); + ASSERT_FALSE(buf.empty()); + + carla::ros2::msg::CarlaLineInvasion recovered{}; + EXPECT_TRUE(carla::ros2::deserialize_from_cdr(buf.data(), buf.size(), recovered)); + EXPECT_EQ(recovered.header.frame_id, "vehicle"); + ASSERT_EQ(recovered.crossed_lane_markings.size(), 3u); + EXPECT_EQ(recovered.crossed_lane_markings[0], 1); + EXPECT_EQ(recovered.crossed_lane_markings[1], 4); + EXPECT_EQ(recovered.crossed_lane_markings[2], 7); +} + +TEST(cdr_serialization, clock_round_trip) { + carla::ros2::msg::Clock original{}; + original.clock.sec = 999; + original.clock.nanosec = 1u; + + auto buf = carla::ros2::serialize_to_cdr(original); + ASSERT_FALSE(buf.empty()); + + carla::ros2::msg::Clock recovered{}; + EXPECT_TRUE(carla::ros2::deserialize_from_cdr(buf.data(), buf.size(), recovered)); + EXPECT_EQ(recovered.clock.sec, 999); + EXPECT_EQ(recovered.clock.nanosec, 1u); +} + +TEST(cdr_serialization, empty_tfmessage_round_trip) { + carla::ros2::msg::TFMessage original{}; + + auto buf = carla::ros2::serialize_to_cdr(original); + ASSERT_FALSE(buf.empty()); + + carla::ros2::msg::TFMessage recovered{}; + EXPECT_TRUE(carla::ros2::deserialize_from_cdr(buf.data(), buf.size(), recovered)); + EXPECT_TRUE(recovered.transforms.empty()); +} + +TEST(cdr_serialization, pointcloud2_multi_field_round_trip) { + // Exercises the manual sequence loop in serialize_cdr/deserialize_cdr for + // PointCloud2::fields with more than one element. The single-field + // round-trip above does not catch a bug in the loop step. + carla::ros2::msg::PointCloud2 original{}; + original.header.frame_id = "lidar"; + original.height = 1u; + original.width = 4u; + + carla::ros2::msg::PointField fx{}; + fx.name = "x"; + fx.offset = 0u; + fx.datatype = carla::ros2::msg::PointField::FLOAT32; + fx.count = 1u; + + carla::ros2::msg::PointField fy{}; + fy.name = "y"; + fy.offset = 4u; + fy.datatype = carla::ros2::msg::PointField::FLOAT32; + fy.count = 1u; + + carla::ros2::msg::PointField fz{}; + fz.name = "z"; + fz.offset = 8u; + fz.datatype = carla::ros2::msg::PointField::FLOAT32; + fz.count = 1u; + + original.fields.push_back(fx); + original.fields.push_back(fy); + original.fields.push_back(fz); + original.is_bigendian = false; + original.point_step = 12u; + original.row_step = 48u; + original.data.resize(48u, 0u); + original.is_dense = true; + + auto buf = carla::ros2::serialize_to_cdr(original); + ASSERT_FALSE(buf.empty()); + + carla::ros2::msg::PointCloud2 recovered{}; + EXPECT_TRUE(carla::ros2::deserialize_from_cdr(buf.data(), buf.size(), recovered)); + ASSERT_EQ(recovered.fields.size(), 3u); + EXPECT_EQ(recovered.fields[0].name, "x"); + EXPECT_EQ(recovered.fields[0].offset, 0u); + EXPECT_EQ(recovered.fields[1].name, "y"); + EXPECT_EQ(recovered.fields[1].offset, 4u); + EXPECT_EQ(recovered.fields[2].name, "z"); + EXPECT_EQ(recovered.fields[2].offset, 8u); + EXPECT_EQ(recovered.point_step, 12u); + EXPECT_EQ(recovered.row_step, 48u); + ASSERT_EQ(recovered.data.size(), 48u); +} + +TEST(cdr_serialization, tfmessage_multi_transform_round_trip) { + // Exercises the manual sequence loop for TFMessage::transforms with more + // than one element. + carla::ros2::msg::TFMessage original{}; + + carla::ros2::msg::TransformStamped a{}; + a.header.stamp.sec = 1; + a.header.frame_id = "world"; + a.child_frame_id = "robot_a"; + a.transform.translation.x = 1.0; + a.transform.rotation.w = 1.0; + + carla::ros2::msg::TransformStamped b{}; + b.header.stamp.sec = 2; + b.header.frame_id = "world"; + b.child_frame_id = "robot_b"; + b.transform.translation.y = 2.0; + b.transform.rotation.w = 1.0; + + original.transforms.push_back(a); + original.transforms.push_back(b); + + auto buf = carla::ros2::serialize_to_cdr(original); + ASSERT_FALSE(buf.empty()); + + carla::ros2::msg::TFMessage recovered{}; + EXPECT_TRUE(carla::ros2::deserialize_from_cdr(buf.data(), buf.size(), recovered)); + ASSERT_EQ(recovered.transforms.size(), 2u); + EXPECT_EQ(recovered.transforms[0].child_frame_id, "robot_a"); + EXPECT_DOUBLE_EQ(recovered.transforms[0].transform.translation.x, 1.0); + EXPECT_EQ(recovered.transforms[1].child_frame_id, "robot_b"); + EXPECT_DOUBLE_EQ(recovered.transforms[1].transform.translation.y, 2.0); +} + +TEST(cdr_serialization, deserialize_truncated_returns_false) { + // A truncated buffer must produce a clean false return, not an uncaught + // Fast-CDR exception leaking out of the bool API. + carla::ros2::msg::Header original{}; + original.stamp.sec = 7; + original.frame_id = "needs_more_bytes"; + + auto buf = carla::ros2::serialize_to_cdr(original); + ASSERT_GT(buf.size(), 8u); + + carla::ros2::msg::Header recovered{}; + EXPECT_FALSE(carla::ros2::deserialize_from_cdr( + buf.data(), buf.size() / 2u, recovered)); +} + +TEST(cdr_serialization, deserialize_corrupt_encapsulation_returns_false) { + // A buffer too small to even hold the 4-byte encapsulation header must + // produce a clean false return. + const uint8_t bogus[2] = {0xFFu, 0xFFu}; + carla::ros2::msg::Time recovered{}; + EXPECT_FALSE(carla::ros2::deserialize_from_cdr(bogus, sizeof(bogus), recovered)); +} + +TEST(cdr_serialization, deserialize_pointcloud2_hostile_length_returns_false) { + // Hand-craft a PointCloud2 buffer that claims its sequence has a hostile + // length (max uint32). Without the kMaxCdrSequenceElements cap, the + // call would attempt a multi-GB resize and abort the process. + std::vector buf; + // Encapsulation header: CDR_LE + options. + buf.push_back(0x00u); + buf.push_back(0x01u); + buf.push_back(0x00u); + buf.push_back(0x00u); + // Header.stamp.sec (int32) + nanosec (uint32) = 8 bytes of zeros. + for (int i = 0; i < 8; ++i) buf.push_back(0x00u); + // Header.frame_id (string): length 1 (NUL only) + "\0" + 3 padding bytes + // to keep alignment for the next uint32. + buf.push_back(0x01u); + buf.push_back(0x00u); + buf.push_back(0x00u); + buf.push_back(0x00u); + buf.push_back(0x00u); + buf.push_back(0x00u); + buf.push_back(0x00u); + buf.push_back(0x00u); + // PointCloud2.height (uint32) + width (uint32). + for (int i = 0; i < 8; ++i) buf.push_back(0x00u); + // fields_size = 0xFFFFFFFF (hostile). + buf.push_back(0xFFu); + buf.push_back(0xFFu); + buf.push_back(0xFFu); + buf.push_back(0xFFu); + + carla::ros2::msg::PointCloud2 recovered{}; + EXPECT_FALSE(carla::ros2::deserialize_from_cdr( + buf.data(), buf.size(), recovered)); +} + +TEST(cdr_serialization, deserialize_tfmessage_hostile_length_returns_false) { + // Same idea for TFMessage::transforms — claim a 4-billion-element + // sequence and verify the cap rejects it instead of OOM-aborting. + std::vector buf; + buf.push_back(0x00u); + buf.push_back(0x01u); + buf.push_back(0x00u); + buf.push_back(0x00u); + buf.push_back(0xFFu); + buf.push_back(0xFFu); + buf.push_back(0xFFu); + buf.push_back(0xFFu); + + carla::ros2::msg::TFMessage recovered{}; + EXPECT_FALSE(carla::ros2::deserialize_from_cdr( + buf.data(), buf.size(), recovered)); +} + +TEST(cdr_serialization, cdr_serialized_size_matches_serialize_to_cdr) { + // cdr_serialized_size(msg) must return the same byte count as + // serialize_to_cdr(msg).size() for every message type. This is the contract + // that GenericCdrPubSubType::getSerializedSizeProvider relies on. + { + carla::ros2::msg::Header msg{}; + msg.stamp.sec = 42; + msg.frame_id = "map"; + EXPECT_EQ(carla::ros2::cdr_serialized_size(msg), + carla::ros2::serialize_to_cdr(msg).size()); + } + { + carla::ros2::msg::Image msg{}; + msg.height = 2u; + msg.width = 3u; + msg.encoding = "rgb8"; + msg.data.assign(6u, 0xAAu); + EXPECT_EQ(carla::ros2::cdr_serialized_size(msg), + carla::ros2::serialize_to_cdr(msg).size()); + } + { + carla::ros2::msg::PointCloud2 msg{}; + msg.height = 1u; + msg.width = 4u; + msg.data.assign(48u, 0xBBu); + EXPECT_EQ(carla::ros2::cdr_serialized_size(msg), + carla::ros2::serialize_to_cdr(msg).size()); + } + { + carla::ros2::msg::TFMessage msg{}; + msg.transforms.resize(2u); + msg.transforms[0].header.frame_id = "world"; + msg.transforms[1].header.frame_id = "base_link"; + EXPECT_EQ(carla::ros2::cdr_serialized_size(msg), + carla::ros2::serialize_to_cdr(msg).size()); + } +} + +TEST(cdr_serialization, cdr_serialized_size_image_exceeds_static_max) { + // A real 800x600 RGB camera frame is ~1.4 MB. The static + // CdrTopicInfo::max_serialized_size() is only 648 bytes. + // cdr_serialized_size() must return a value greater than the static max, + // and the round-trip must recover the original data.size(). + carla::ros2::msg::Image msg{}; + msg.height = 600u; + msg.width = 800u; + msg.encoding = "rgb8"; + msg.step = 800u * 3u; + const size_t data_bytes = 800u * 600u * 3u; // 1,440,000 bytes + msg.data.assign(data_bytes, 0x7Fu); + + const uint32_t computed = carla::ros2::cdr_serialized_size(msg); + EXPECT_GT(computed, + static_cast( + carla::ros2::CdrTopicInfo::max_serialized_size())); + + const auto bytes = carla::ros2::serialize_to_cdr(msg); + ASSERT_FALSE(bytes.empty()); + EXPECT_EQ(computed, static_cast(bytes.size())); + + carla::ros2::msg::Image recovered{}; + ASSERT_TRUE(carla::ros2::deserialize_from_cdr( + bytes.data(), bytes.size(), recovered)); + EXPECT_EQ(recovered.data.size(), data_bytes); +} + +TEST(cdr_serialization, cdr_serialized_size_pointcloud2_exceeds_static_max) { + // A typical LiDAR scan is 1-20 MB. The static max_serialized_size() for + // PointCloud2 is 27597 bytes. This test uses ~1 MB of data to verify the + // same contract as the Image test above. + carla::ros2::msg::PointCloud2 msg{}; + msg.height = 1u; + msg.width = 22000u; + msg.row_step = 22000u * 16u; + const size_t data_bytes = 22000u * 16u; // ~352,000 bytes (~0.35 MB) + msg.data.assign(data_bytes, 0x3Cu); + carla::ros2::msg::PointField pf{}; + pf.name = "x"; + pf.offset = 0u; + pf.datatype = 7u; // FLOAT32 + pf.count = 1u; + msg.fields.push_back(pf); + + const uint32_t computed = carla::ros2::cdr_serialized_size(msg); + EXPECT_GT(computed, + static_cast( + carla::ros2::CdrTopicInfo::max_serialized_size())); + + const auto bytes = carla::ros2::serialize_to_cdr(msg); + ASSERT_FALSE(bytes.empty()); + EXPECT_EQ(computed, static_cast(bytes.size())); + + carla::ros2::msg::PointCloud2 recovered{}; + ASSERT_TRUE(carla::ros2::deserialize_from_cdr( + bytes.data(), bytes.size(), recovered)); + EXPECT_EQ(recovered.data.size(), data_bytes); +} diff --git a/Ros2Native/CMakeLists.txt b/Ros2Native/CMakeLists.txt index fb60dd25983..f62c76dabfa 100644 --- a/Ros2Native/CMakeLists.txt +++ b/Ros2Native/CMakeLists.txt @@ -42,6 +42,8 @@ ExternalProject_add ( -DOPENSSL_INCLUDE_DIR:FILEPATH=${UE_OPENSSL_INCLUDE} -DOPENSSL_SSL_LIBRARY:FILEPATH=${UE_OPENSSL_LIBS}/libssl.a -DOPENSSL_CRYPTO_LIBRARY:FILEPATH=${UE_OPENSSL_LIBS}/libcrypto.a + INSTALL_BYPRODUCTS + ${PROJECT_INSTALL_PATH}/lib/libfastcdr.so DEPENDS foonathan_memory ) From f6d9a5b31958c53313c45eb4cb2f5151c20e8060 Mon Sep 17 00:00:00 2001 From: Jesus Armando Anaya Date: Tue, 7 Jul 2026 16:22:57 -0700 Subject: [PATCH 4/7] feat(ros2): add middleware abstraction and FastDDS-on-CDR middleware Introduce the vendor-neutral middleware strategy layer of the ROS 2 middleware decoupling series and its FastDDS implementation, compiled into libcarla-ros2-native.so. Ported from ue4-dev: - middleware/Middleware.h: the Middleware enum plus the string and ROS 2 type-name helpers; the CycloneDDS value and its availability branches are present but stay compiled out until the CycloneDDS middleware lands (upstream ue4-dev #9608). - middleware/IPublisherMiddleware.h, ISubscriberMiddleware.h: the type-erased publisher/subscriber strategy interfaces. Subscribers write received samples straight into caller-owned storage to avoid a copy (upstream ue4-dev #9608). - middleware/MiddlewareFactory.h: creates the active middleware for a traits type; each vendor arm is double-gated on its CARLA_ROS2_MIDDLEWARE_* macro and CARLA_ROS2_MIDDLEWARE_TESTING so the suite exercises the availability logic without linking DDS (upstream ue4-dev #9608). - middleware/ActiveMiddleware.{h,cpp}: a DDS-free bridge (SetActiveMiddleware) so ROS2.cpp, the only ROS 2 translation unit in carla-server, can select the middleware without any DDS header crossing the shared-library boundary. Nothing calls it until the cutover; the definition ships now so the shared lib has a translation unit that compiles MiddlewareFactory.h and the FastDDS headers with the real vendor macros. - middleware/fastdds/GenericCdrPubSubType.h: one FastDDS TopicDataType that serializes every carla::ros2::msg::* struct through the unified CdrSerialization.h path, replacing the generated per-type PubSubType classes; getSerializedSizeProvider reports the actual instance size so variable-length payloads (camera frames, point clouds) are not bounded by the static max size (upstream ue4-dev #9643). - middleware/fastdds/FastDDS{Publisher,Subscriber}Middleware.h: the FastDDS strategy implementations. Each endpoint advertises the REP-2016 "typehash=RIHS01_;" USER_DATA so Jazzy RMWs match on the REP-2011 type hash (upstream ue4-dev #9681). - middleware/fastdds/FastDDSSharedParticipant.{h,cpp}: a refcounted process-wide DomainParticipant shared across all FastDDS endpoints, avoiding the discovery storm that destroying N participants back to back caused on shutdown (upstream ue4-dev #9681). UE5 adaptation: ue5-dev pins FastDDS 2.11.2 with bundled Fast-CDR 1.x, so GenericCdrPubSubType.h keeps the Fast-CDR 1.x spellings (eprosima::fastcdr::Cdr::DDS_CDR, getSerializedDataLength()) matching CdrSerialization.h from the previous PR of the series. The wire format is classic CDR little-endian and is unchanged. The existing publishers keep using the generated FastDDS types; the cutover to this abstraction and the PublisherImpl/SubscriberImpl rewrite land in the next PR of the series. No behavior change. (adapted from ue4-dev 82c28e28b6b60fcd71ef550fe9b6cd0e678c2794) (adapted from ue4-dev f53144c97f5f1225606b9d4f8be4e35af1c7d5f8) (adapted from ue4-dev 02a83ef8cf69adaf9f23fb6efb9f1b06211d2733) (adapted from ue4-dev c64e8f49d44b35f9e1782caaea546a361600698b) --- .../ros2/middleware/ActiveMiddleware.cpp | 16 ++ .../carla/ros2/middleware/ActiveMiddleware.h | 24 +++ .../ros2/middleware/IPublisherMiddleware.h | 37 ++++ .../ros2/middleware/ISubscriberMiddleware.h | 38 ++++ .../source/carla/ros2/middleware/Middleware.h | 80 ++++++++ .../carla/ros2/middleware/MiddlewareFactory.h | 142 ++++++++++++++ .../fastdds/FastDDSPublisherMiddleware.h | 180 ++++++++++++++++++ .../fastdds/FastDDSSharedParticipant.cpp | 83 ++++++++ .../fastdds/FastDDSSharedParticipant.h | 60 ++++++ .../fastdds/FastDDSSubscriberMiddleware.h | 180 ++++++++++++++++++ .../middleware/fastdds/GenericCdrPubSubType.h | 151 +++++++++++++++ Ros2Native/LibCarlaRos2Native/CMakeLists.txt | 6 + 12 files changed, 997 insertions(+) create mode 100644 LibCarla/source/carla/ros2/middleware/ActiveMiddleware.cpp create mode 100644 LibCarla/source/carla/ros2/middleware/ActiveMiddleware.h create mode 100644 LibCarla/source/carla/ros2/middleware/IPublisherMiddleware.h create mode 100644 LibCarla/source/carla/ros2/middleware/ISubscriberMiddleware.h create mode 100644 LibCarla/source/carla/ros2/middleware/Middleware.h create mode 100644 LibCarla/source/carla/ros2/middleware/MiddlewareFactory.h create mode 100644 LibCarla/source/carla/ros2/middleware/fastdds/FastDDSPublisherMiddleware.h create mode 100644 LibCarla/source/carla/ros2/middleware/fastdds/FastDDSSharedParticipant.cpp create mode 100644 LibCarla/source/carla/ros2/middleware/fastdds/FastDDSSharedParticipant.h create mode 100644 LibCarla/source/carla/ros2/middleware/fastdds/FastDDSSubscriberMiddleware.h create mode 100644 LibCarla/source/carla/ros2/middleware/fastdds/GenericCdrPubSubType.h diff --git a/LibCarla/source/carla/ros2/middleware/ActiveMiddleware.cpp b/LibCarla/source/carla/ros2/middleware/ActiveMiddleware.cpp new file mode 100644 index 00000000000..da6534b79e6 --- /dev/null +++ b/LibCarla/source/carla/ros2/middleware/ActiveMiddleware.cpp @@ -0,0 +1,16 @@ +// 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 . + +#include "carla/ros2/middleware/ActiveMiddleware.h" +#include "carla/ros2/middleware/MiddlewareFactory.h" + +namespace carla { +namespace ros2 { + +void SetActiveMiddleware(Middleware middleware) { + MiddlewareFactory::SetMiddleware(middleware); +} + +} // namespace ros2 +} // namespace carla diff --git a/LibCarla/source/carla/ros2/middleware/ActiveMiddleware.h b/LibCarla/source/carla/ros2/middleware/ActiveMiddleware.h new file mode 100644 index 00000000000..e3292631bbc --- /dev/null +++ b/LibCarla/source/carla/ros2/middleware/ActiveMiddleware.h @@ -0,0 +1,24 @@ +// 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 . + +#pragma once + +#include "carla/ros2/middleware/Middleware.h" + +namespace carla { +namespace ros2 { + +/// DDS-free bridge for selecting the active middleware across the shared-library +/// boundary. +/// +/// ROS2.cpp is the only ROS 2 translation unit compiled into carla-server; every +/// other ROS 2 source (including MiddlewareFactory.h and the vendor middleware +/// headers) is compiled exclusively into libcarla-ros2-native.so. This declaration +/// includes only Middleware.h, so carla-server can select the middleware without +/// any DDS header crossing the boundary. The definition (which forwards to +/// MiddlewareFactory::SetMiddleware) is compiled into the shared library. +void SetActiveMiddleware(Middleware middleware); + +} // namespace ros2 +} // namespace carla diff --git a/LibCarla/source/carla/ros2/middleware/IPublisherMiddleware.h b/LibCarla/source/carla/ros2/middleware/IPublisherMiddleware.h new file mode 100644 index 00000000000..623525bdf83 --- /dev/null +++ b/LibCarla/source/carla/ros2/middleware/IPublisherMiddleware.h @@ -0,0 +1,37 @@ +// 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 . + +#pragma once + +#include + +namespace carla { +namespace ros2 { + +/// Type-erased abstract interface for a publisher middleware. +/// Concrete implementations handle all vendor-specific entity creation, +/// type registration, and data writing. +class IPublisherMiddleware { + public: + virtual ~IPublisherMiddleware() = default; + + /// Initialize the underlying middleware entities. + /// @param topic_name Full topic name including "rt/" prefix. + /// @return true on success. + virtual bool Init(const std::string& topic_name) = 0; + + /// Serialize and write a message to the network. + /// @param message_data Pointer to the message object (type-erased, cast internally). + /// @return true if the write succeeded. + virtual bool Publish(void* message_data) = 0; + + /// @return true if at least one subscriber is matched. + virtual bool IsAlive() const = 0; + + /// @return The topic name this publisher is bound to. + virtual std::string GetTopicName() const = 0; +}; + +} // namespace ros2 +} // namespace carla diff --git a/LibCarla/source/carla/ros2/middleware/ISubscriberMiddleware.h b/LibCarla/source/carla/ros2/middleware/ISubscriberMiddleware.h new file mode 100644 index 00000000000..414aa20af4c --- /dev/null +++ b/LibCarla/source/carla/ros2/middleware/ISubscriberMiddleware.h @@ -0,0 +1,38 @@ +// 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 . + +#pragma once + +#include + +namespace carla { +namespace ros2 { + +/// Type-erased abstract interface for a subscriber middleware. +/// Concrete implementations write received messages directly into the caller-provided +/// storage (message_ptr / new_message_flag) to avoid an extra copy. +class ISubscriberMiddleware { + public: + virtual ~ISubscriberMiddleware() = default; + + /// Initialize the underlying middleware entities. + /// The middleware writes incoming messages to *message_ptr and sets *new_message_flag = true. + /// @param topic_name Full topic name. + /// @param message_ptr Pointer to the message storage owned by the caller. + /// @param new_message_flag Pointer to the new-message flag owned by the caller. + /// @return true on success. + virtual bool Init( + const std::string& topic_name, + void* message_ptr, + bool* new_message_flag) = 0; + + /// @return true if at least one publisher is matched. + virtual bool IsAlive() const = 0; + + /// @return The topic name this subscriber is bound to. + virtual std::string GetTopicName() const = 0; +}; + +} // namespace ros2 +} // namespace carla diff --git a/LibCarla/source/carla/ros2/middleware/Middleware.h b/LibCarla/source/carla/ros2/middleware/Middleware.h new file mode 100644 index 00000000000..85366ec041e --- /dev/null +++ b/LibCarla/source/carla/ros2/middleware/Middleware.h @@ -0,0 +1,80 @@ +// 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 . + +#pragma once + +#include + +namespace carla { +namespace ros2 { + +/// Enumeration of available middleware implementations. +/// Passed to ROS2::Enable() to select the middleware at startup. +/// Once set, the middleware cannot be changed without restarting. +enum class Middleware { + FastDDS, + CycloneDDS +}; + +/// Convert a Middleware enum value to a readable string. +inline const char* MiddlewareToString(Middleware middleware) { + switch (middleware) { + case Middleware::FastDDS: + return "FastDDS"; + case Middleware::CycloneDDS: + return "CycloneDDS"; + } + return "Unknown"; +} + +/// Result of parsing a middleware name string. +struct MiddlewareParseResult { + bool valid; + Middleware middleware; +}; + +/// Parse a middleware name string (lowercase). Returns {true, middleware} on match, +/// {false, FastDDS} for unrecognized values. +inline MiddlewareParseResult MiddlewareFromString(const std::string& name) { + if (name == "fastdds") { + return {true, Middleware::FastDDS}; + } + if (name == "cyclonedds") { + return {true, Middleware::CycloneDDS}; + } + return {false, Middleware::FastDDS}; +} + +/// Return a readable list of middleware implementations compiled into this binary. +inline std::string GetAvailableMiddlewareString() { + std::string result; +#if defined(CARLA_ROS2_MIDDLEWARE_FASTDDS) + result += "FastDDS"; +#endif +#if defined(CARLA_ROS2_MIDDLEWARE_CYCLONEDDS) + if (!result.empty()) { + result += ", "; + } + result += "CycloneDDS"; +#endif + if (result.empty()) { + result = "none"; + } + return result; +} + +/// Mangle a 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 ToROS2TypeName(const std::string& type_name) { + auto pos = type_name.rfind("::"); + if (pos == std::string::npos) { + return "dds_::" + type_name + "_"; + } + return type_name.substr(0, pos) + + "::dds_::" + type_name.substr(pos + 2) + "_"; +} + +} // namespace ros2 +} // namespace carla diff --git a/LibCarla/source/carla/ros2/middleware/MiddlewareFactory.h b/LibCarla/source/carla/ros2/middleware/MiddlewareFactory.h new file mode 100644 index 00000000000..eb40f63cb3f --- /dev/null +++ b/LibCarla/source/carla/ros2/middleware/MiddlewareFactory.h @@ -0,0 +1,142 @@ +// 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 . + +#pragma once + +#include +#include + +#include "carla/ros2/middleware/Middleware.h" +#include "carla/ros2/middleware/IPublisherMiddleware.h" +#include "carla/ros2/middleware/ISubscriberMiddleware.h" +#include "carla/Logging.h" + +#if defined(CARLA_ROS2_MIDDLEWARE_FASTDDS) && !defined(CARLA_ROS2_MIDDLEWARE_TESTING) +# include "carla/ros2/middleware/fastdds/FastDDSPublisherMiddleware.h" +# include "carla/ros2/middleware/fastdds/FastDDSSubscriberMiddleware.h" +#endif + +#if defined(CARLA_ROS2_MIDDLEWARE_CYCLONEDDS) && !defined(CARLA_ROS2_MIDDLEWARE_TESTING) +# include "carla/ros2/middleware/cyclonedds/CycloneDDSPublisherMiddleware.h" +# include "carla/ros2/middleware/cyclonedds/CycloneDDSSubscriberMiddleware.h" +#endif + +namespace carla { +namespace ros2 { + +/// Factory that creates publisher/subscriber middleware based on the active middleware selection. +/// The middleware is set once at startup via SetMiddleware() before any entities are created. +/// After the first entity is created, changing the middleware has undefined behavior. +class MiddlewareFactory { + public: + /// Select the middleware for all subsequent publisher/subscriber creation. + /// Must be called before any publisher or subscriber is initialized. + static void SetMiddleware(Middleware middleware) { + GetActiveMiddleware() = middleware; + } + + /// @return The currently selected middleware. + static Middleware GetMiddleware() { + return GetActiveMiddleware(); + } + + /// Check whether a specific middleware was compiled into this binary. + static bool IsMiddlewareAvailable(Middleware middleware) { + switch (middleware) { + case Middleware::FastDDS: +#if defined(CARLA_ROS2_MIDDLEWARE_FASTDDS) + return true; +#else + return false; +#endif + case Middleware::CycloneDDS: +#if defined(CARLA_ROS2_MIDDLEWARE_CYCLONEDDS) + return true; +#else + return false; +#endif + } + return false; + } + + /// Result of middleware resolution — whether resolution succeeded and which middleware to use. + struct MiddlewareResolution { + bool success; + Middleware middleware; + }; + + /// Resolve the requested middleware strictly — no fallback to other middleware. + /// Returns {true, requested} if available, {false, requested} otherwise. + static MiddlewareResolution ResolveMiddleware(Middleware requested) { + if (IsMiddlewareAvailable(requested)) { + return {true, requested}; + } + return {false, requested}; + } + + /// Return a readable list of middleware implementations compiled into this binary. + /// Delegates to the free function in Middleware.h. + static std::string GetAvailableMiddlewareString() { + return carla::ros2::GetAvailableMiddlewareString(); + } + + /// Create a publisher middleware for traits type T. + /// T must provide: + /// T::msg_type — the message type + template + static std::unique_ptr CreatePublisher() { + switch (GetActiveMiddleware()) { + case Middleware::FastDDS: +#if defined(CARLA_ROS2_MIDDLEWARE_FASTDDS) && !defined(CARLA_ROS2_MIDDLEWARE_TESTING) + return std::make_unique>(); +#else + log_error("MiddlewareFactory: FastDDS not compiled in"); + return nullptr; +#endif + case Middleware::CycloneDDS: +#if defined(CARLA_ROS2_MIDDLEWARE_CYCLONEDDS) && !defined(CARLA_ROS2_MIDDLEWARE_TESTING) + return std::make_unique>(); +#else + log_error("MiddlewareFactory: CycloneDDS not compiled in"); + return nullptr; +#endif + } + return nullptr; + } + + /// Create a subscriber middleware for traits type S. + /// S must provide: + /// S::msg_type — the message type + template + static std::unique_ptr CreateSubscriber() { + switch (GetActiveMiddleware()) { + case Middleware::FastDDS: +#if defined(CARLA_ROS2_MIDDLEWARE_FASTDDS) && !defined(CARLA_ROS2_MIDDLEWARE_TESTING) + return std::make_unique>(); +#else + log_error("MiddlewareFactory: FastDDS not compiled in"); + return nullptr; +#endif + case Middleware::CycloneDDS: +#if defined(CARLA_ROS2_MIDDLEWARE_CYCLONEDDS) && !defined(CARLA_ROS2_MIDDLEWARE_TESTING) + return std::make_unique>(); +#else + log_error("MiddlewareFactory: CycloneDDS not compiled in"); + return nullptr; +#endif + } + return nullptr; + } + + private: + /// Returns reference to the active middleware selection + /// (function-local static for C++11 thread safety). + static Middleware& GetActiveMiddleware() { + static Middleware active_middleware = Middleware::FastDDS; + return active_middleware; + } +}; + +} // namespace ros2 +} // namespace carla diff --git a/LibCarla/source/carla/ros2/middleware/fastdds/FastDDSPublisherMiddleware.h b/LibCarla/source/carla/ros2/middleware/fastdds/FastDDSPublisherMiddleware.h new file mode 100644 index 00000000000..5184ae16ad8 --- /dev/null +++ b/LibCarla/source/carla/ros2/middleware/fastdds/FastDDSPublisherMiddleware.h @@ -0,0 +1,180 @@ +// 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 . + +#pragma once + +#include "carla/ros2/middleware/IPublisherMiddleware.h" +#include "carla/ros2/middleware/fastdds/FastDDSSharedParticipant.h" +#include "carla/ros2/middleware/fastdds/GenericCdrPubSubType.h" +#include "carla/ros2/types/UserDataFormat.h" +#include "carla/Logging.h" + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +namespace carla { +namespace ros2 { + +namespace efd = eprosima::fastdds::dds; +using erc = eprosima::fastrtps::types::ReturnCode_t; + +/// FastDDS implementation of IPublisherMiddleware. +/// Parameterized on a traits type T that provides: +/// T::msg_type — the message type (a carla::ros2::msg::* POD struct) +/// Serialization is handled by GenericCdrPubSubType via CdrSerialization.h. +/// +/// Uses FastDDSSharedParticipant for a single refcounted DomainParticipant +/// across all FastDDS endpoints, mirroring CycloneDDS's shared-participant +/// model. This avoids the discovery storm that occurred when N independent +/// participants were destroyed back-to-back on ROS2::Shutdown(). +/// +/// Sets USER_DATA QoS on the DataWriter to the REP-2016 KV payload +/// "typehash=RIHS01_;" (PID_USER_DATA = 0x002c in DDSI-RTPS v2.5 +/// §9.6.2.2.2) so that Jazzy rmw_cyclonedds_cpp and rmw_fastrtps_cpp can +/// perform REP-2011 type-hash-based endpoint matching without warning. +template +class FastDDSPublisherMiddleware + : public IPublisherMiddleware, + public eprosima::fastdds::dds::DataWriterListener { + public: + using msg_type = typename T::msg_type; + + FastDDSPublisherMiddleware() = default; + FastDDSPublisherMiddleware(const FastDDSPublisherMiddleware&) = delete; + FastDDSPublisherMiddleware& operator=(const FastDDSPublisherMiddleware&) = delete; + + void on_publication_matched( + efd::DataWriter* writer, + const efd::PublicationMatchedStatus& info) override { + _alive.store(info.current_count > 0, std::memory_order_relaxed); + } + + ~FastDDSPublisherMiddleware() override { + if (_datawriter) { + // Detach listener before deletion so that on_publication_matched cannot + // fire on a partially-destroyed object. + _datawriter->set_listener(nullptr); + _publisher->delete_datawriter(_datawriter); + } + if (_publisher) { + _participant->delete_publisher(_publisher); + } + if (_topic) { + _participant->delete_topic(_topic); + } + if (_participant) { + // Release the per-type refcount so unregister_type() fires when the last + // user of this TypeSupport goes away, before the shared participant drops. + FastDDSSharedParticipant::release_type(_type->getName()); + FastDDSSharedParticipant::release(); + _participant = nullptr; + } + } + + bool Init(const std::string& topic_name) override { + if (_type == nullptr) { + log_error("FastDDSPublisherMiddleware: Invalid TypeSupport"); + return false; + } + + _participant = FastDDSSharedParticipant::acquire(); + if (_participant == nullptr) { + log_error("FastDDSPublisherMiddleware: Shared participant unavailable"); + return false; + } + + _type.register_type(_participant); + FastDDSSharedParticipant::retain_type(_type->getName()); + + efd::PublisherQos pubqos = efd::PUBLISHER_QOS_DEFAULT; + _publisher = _participant->create_publisher(pubqos, nullptr); + if (_publisher == nullptr) { + log_error("FastDDSPublisherMiddleware: Failed to create Publisher"); + return false; + } + + // Multiple endpoints may share the same topic on the shared participant + // (e.g. every actor publishes on rt/tf). FastDDS's create_topic rejects a + // duplicate name, so probe first and reuse via find_topic if it already + // exists; find_topic increments an internal refcount that the matching + // delete_topic in the destructor balances. + if (_participant->lookup_topicdescription(topic_name) != nullptr) { + _topic = _participant->find_topic(topic_name, eprosima::fastrtps::Duration_t{0, 0}); + } else { + efd::TopicQos tqos = efd::TOPIC_QOS_DEFAULT; + _topic = _participant->create_topic(topic_name, _type->getName(), tqos); + } + if (_topic == nullptr) { + log_error("FastDDSPublisherMiddleware: Failed to create Topic"); + return false; + } + + efd::DataWriterQos wqos = efd::DATAWRITER_QOS_DEFAULT; + wqos.endpoint().history_memory_policy = + eprosima::fastrtps::rtps::PREALLOCATED_WITH_REALLOC_MEMORY_MODE; + + // 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_;". + // Jazzy rmw_cyclonedds_cpp / rmw_fastrtps_cpp parse this during SEDP + // endpoint discovery to verify type compatibility. + auto ud = build_user_data_for(); + if (!ud.empty()) { + wqos.user_data().data_vec(ud); + } + + efd::DataWriterListener* listener = + static_cast(this); + _datawriter = _publisher->create_datawriter(_topic, wqos, listener); + if (_datawriter == nullptr) { + log_error("FastDDSPublisherMiddleware: Failed to create DataWriter"); + return false; + } + + _topic_name = topic_name; + return true; + } + + bool Publish(void* message_data) override { + eprosima::fastrtps::rtps::InstanceHandle_t instance_handle; + erc rcode = _datawriter->write(message_data, instance_handle); + if (rcode == erc::ReturnCodeValue::RETCODE_OK) { + return true; + } + log_error("FastDDSPublisherMiddleware::Publish (", + _topic_name, ") failed with code:", rcode()); + return false; + } + + bool IsAlive() const override { + return _alive.load(std::memory_order_relaxed); + } + + std::string GetTopicName() const override { + return _topic_name; + } + + private: + efd::DomainParticipant* _participant { nullptr }; + efd::Publisher* _publisher { nullptr }; + efd::Topic* _topic { nullptr }; + efd::DataWriter* _datawriter { nullptr }; + efd::TypeSupport _type { new GenericCdrPubSubType() }; + + std::string _topic_name; + std::atomic _alive { false }; +}; + +} // namespace ros2 +} // namespace carla diff --git a/LibCarla/source/carla/ros2/middleware/fastdds/FastDDSSharedParticipant.cpp b/LibCarla/source/carla/ros2/middleware/fastdds/FastDDSSharedParticipant.cpp new file mode 100644 index 00000000000..c4a6558f72c --- /dev/null +++ b/LibCarla/source/carla/ros2/middleware/fastdds/FastDDSSharedParticipant.cpp @@ -0,0 +1,83 @@ +// 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 . + +#ifndef CARLA_ROS2_MIDDLEWARE_TESTING + +#include "carla/ros2/middleware/fastdds/FastDDSSharedParticipant.h" +#include "carla/Logging.h" + +#include +#include + +namespace carla { +namespace ros2 { + +namespace efd = eprosima::fastdds::dds; + +// Static member definitions +std::mutex FastDDSSharedParticipant::_mutex; +efd::DomainParticipant* FastDDSSharedParticipant::_participant { nullptr }; +uint32_t FastDDSSharedParticipant::_refcount { 0u }; +std::unordered_map FastDDSSharedParticipant::_type_refcounts; + +efd::DomainParticipant* FastDDSSharedParticipant::acquire() { + std::lock_guard lock(_mutex); + if (_refcount == 0u) { + efd::DomainParticipantQos pqos = efd::PARTICIPANT_QOS_DEFAULT; + _participant = + efd::DomainParticipantFactory::get_instance()->create_participant(0, pqos); + if (_participant == nullptr) { + log_error("FastDDSSharedParticipant: Failed to create DomainParticipant"); + return nullptr; + } + } + ++_refcount; + return _participant; +} + +void FastDDSSharedParticipant::release() { + std::lock_guard lock(_mutex); + if (_refcount == 0u) { + log_error("FastDDSSharedParticipant::release() called with refcount already 0"); + return; + } + --_refcount; + if (_refcount == 0u && _participant != nullptr) { + // delete_contained_entities() drains all Topics and TypeSupports still + // owned by the participant before the participant itself is deleted. + // Without this call, TypeSupport objects registered with register_type() + // but not explicitly unregistered could be leaked or double-freed. + _participant->delete_contained_entities(); + efd::DomainParticipantFactory::get_instance()->delete_participant(_participant); + _participant = nullptr; + _type_refcounts.clear(); + } +} + +void FastDDSSharedParticipant::retain_type(const std::string& type_name) { + std::lock_guard lock(_mutex); + ++_type_refcounts[type_name]; +} + +void FastDDSSharedParticipant::release_type(const std::string& type_name) { + std::lock_guard lock(_mutex); + auto it = _type_refcounts.find(type_name); + if (it == _type_refcounts.end() || it->second == 0u) { + return; + } + --it->second; + if (it->second == 0u) { + // Unregister the type from the shared participant so the TypeSupport + // refcount inside FastDDS reaches zero cleanly. + if (_participant != nullptr) { + _participant->unregister_type(type_name); + } + _type_refcounts.erase(it); + } +} + +} // namespace ros2 +} // namespace carla + +#endif // !CARLA_ROS2_MIDDLEWARE_TESTING diff --git a/LibCarla/source/carla/ros2/middleware/fastdds/FastDDSSharedParticipant.h b/LibCarla/source/carla/ros2/middleware/fastdds/FastDDSSharedParticipant.h new file mode 100644 index 00000000000..7c00bae999c --- /dev/null +++ b/LibCarla/source/carla/ros2/middleware/fastdds/FastDDSSharedParticipant.h @@ -0,0 +1,60 @@ +// 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 . + +#pragma once + +#ifndef CARLA_ROS2_MIDDLEWARE_TESTING + +#include + +#include +#include +#include +#include + +namespace carla { +namespace ros2 { + +/// Refcounted singleton DomainParticipant shared across all FastDDS +/// publishers and subscribers in the process. +/// +/// Using one DomainParticipant per publisher (the previous approach) caused a +/// discovery storm on ROS2::Shutdown() when all N participants were destroyed +/// back-to-back, triggering intermittent segfaults in remote rmw_fastrtps_cpp +/// and rmw_cyclonedds_cpp clients. CycloneDDS already uses a +/// process-lifetime shared participant (CycloneDDSSertype::carla_cdr_get_participant); +/// this class mirrors that model for FastDDS. +/// +/// Thread-safety: all methods acquire an internal mutex. +class FastDDSSharedParticipant { + public: + /// Increment the refcount and return the shared DomainParticipant. + /// Creates the participant on the first call. Returns nullptr on failure. + static eprosima::fastdds::dds::DomainParticipant* acquire(); + + /// Decrement the refcount. Destroys the participant when count reaches 0. + /// Must be called exactly once for every successful acquire(). + static void release(); + + /// Notify the singleton that one more endpoint is using type_name so that + /// unregister_type() is deferred until the last user releases it. + static void retain_type(const std::string& type_name); + + /// Release one use of type_name. Calls participant->unregister_type() when + /// the count for that name drops to 0 (and the participant is still alive). + static void release_type(const std::string& type_name); + + private: + FastDDSSharedParticipant() = delete; + + static std::mutex _mutex; + static eprosima::fastdds::dds::DomainParticipant* _participant; + static uint32_t _refcount; + static std::unordered_map _type_refcounts; +}; + +} // namespace ros2 +} // namespace carla + +#endif // !CARLA_ROS2_MIDDLEWARE_TESTING diff --git a/LibCarla/source/carla/ros2/middleware/fastdds/FastDDSSubscriberMiddleware.h b/LibCarla/source/carla/ros2/middleware/fastdds/FastDDSSubscriberMiddleware.h new file mode 100644 index 00000000000..1f67357b56f --- /dev/null +++ b/LibCarla/source/carla/ros2/middleware/fastdds/FastDDSSubscriberMiddleware.h @@ -0,0 +1,180 @@ +// 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 . + +#pragma once + +#include "carla/ros2/middleware/ISubscriberMiddleware.h" +#include "carla/ros2/middleware/fastdds/FastDDSSharedParticipant.h" +#include "carla/ros2/middleware/fastdds/GenericCdrPubSubType.h" +#include "carla/ros2/types/UserDataFormat.h" +#include "carla/Logging.h" + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +namespace carla { +namespace ros2 { + +namespace efd = eprosima::fastdds::dds; +using erc = eprosima::fastrtps::types::ReturnCode_t; + +/// FastDDS implementation of ISubscriberMiddleware. +/// Parameterized on traits type S that provides: +/// S::msg_type — the message type (a carla::ros2::msg::* POD struct) +/// Deserialization is handled by GenericCdrPubSubType via CdrSerialization.h. +/// +/// Uses FastDDSSharedParticipant for a single refcounted DomainParticipant +/// across all FastDDS endpoints. See FastDDSPublisherMiddleware.h for the rationale. +/// +/// Sets USER_DATA QoS on the DataReader to the REP-2016 KV payload +/// "typehash=RIHS01_;" (PID_USER_DATA = 0x002c in DDSI-RTPS v2.5 +/// §9.6.2.2.2) so that Jazzy rmw_cyclonedds_cpp and rmw_fastrtps_cpp can +/// perform REP-2011 type-hash-based endpoint matching without warning. +template +class FastDDSSubscriberMiddleware + : public ISubscriberMiddleware, + public eprosima::fastdds::dds::DataReaderListener { + public: + using msg_type = typename S::msg_type; + + FastDDSSubscriberMiddleware() = default; + FastDDSSubscriberMiddleware(const FastDDSSubscriberMiddleware&) = delete; + FastDDSSubscriberMiddleware& operator=(const FastDDSSubscriberMiddleware&) = delete; + + void on_subscription_matched( + efd::DataReader* reader, + const efd::SubscriptionMatchedStatus& info) override { + _alive.store(info.current_count > 0, std::memory_order_relaxed); + } + + void on_data_available(efd::DataReader* reader) override { + efd::SampleInfo info; + erc rcode = reader->take_next_sample(_message_ptr, &info); + if (rcode == erc::ReturnCodeValue::RETCODE_OK) { + *_new_message_ptr = true; + } else { + log_error("FastDDSSubscriberMiddleware::on_data_available (", + _topic_name, ") failed with code:", rcode()); + } + } + + ~FastDDSSubscriberMiddleware() override { + if (_datareader) { + // Detach listener before deletion so that on_data_available cannot + // fire on a partially-destroyed object. + _datareader->set_listener(nullptr); + _subscriber->delete_datareader(_datareader); + } + if (_subscriber) { + _participant->delete_subscriber(_subscriber); + } + if (_topic) { + _participant->delete_topic(_topic); + } + if (_participant) { + FastDDSSharedParticipant::release_type(_type->getName()); + FastDDSSharedParticipant::release(); + _participant = nullptr; + } + } + + bool Init( + const std::string& topic_name, + void* message_ptr, + bool* new_message_flag) override { + _message_ptr = static_cast(message_ptr); + _new_message_ptr = new_message_flag; + + if (_type == nullptr) { + log_error("FastDDSSubscriberMiddleware: Invalid TypeSupport"); + return false; + } + + _participant = FastDDSSharedParticipant::acquire(); + if (_participant == nullptr) { + log_error("FastDDSSubscriberMiddleware: Shared participant unavailable"); + return false; + } + + _type.register_type(_participant); + FastDDSSharedParticipant::retain_type(_type->getName()); + + efd::SubscriberQos subqos = efd::SUBSCRIBER_QOS_DEFAULT; + _subscriber = _participant->create_subscriber(subqos, nullptr); + if (_subscriber == nullptr) { + log_error("FastDDSSubscriberMiddleware: Failed to create Subscriber"); + return false; + } + + // Reuse an existing topic on the shared participant when present: FastDDS + // rejects duplicate create_topic for the same name. find_topic bumps the + // participant's topic refcount, balanced by delete_topic in the destructor. + if (_participant->lookup_topicdescription(topic_name) != nullptr) { + _topic = _participant->find_topic(topic_name, eprosima::fastrtps::Duration_t{0, 0}); + } else { + efd::TopicQos tqos = efd::TOPIC_QOS_DEFAULT; + _topic = _participant->create_topic(topic_name, _type->getName(), tqos); + } + if (_topic == nullptr) { + log_error("FastDDSSubscriberMiddleware: Failed to create Topic"); + return false; + } + + efd::DataReaderQos rqos = efd::DATAREADER_QOS_DEFAULT; + + // 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_;". + auto ud = build_user_data_for(); + if (!ud.empty()) { + rqos.user_data().data_vec(ud); + } + + efd::DataReaderListener* listener = + static_cast(this); + _datareader = _subscriber->create_datareader(_topic, rqos, listener); + if (_datareader == nullptr) { + log_error("FastDDSSubscriberMiddleware: Failed to create DataReader"); + return false; + } + + _topic_name = topic_name; + return true; + } + + bool IsAlive() const override { + return _alive.load(std::memory_order_relaxed); + } + + std::string GetTopicName() const override { + return _topic_name; + } + + private: + efd::DomainParticipant* _participant { nullptr }; + efd::Subscriber* _subscriber { nullptr }; + efd::Topic* _topic { nullptr }; + efd::DataReader* _datareader { nullptr }; + efd::TypeSupport _type { new GenericCdrPubSubType() }; + + msg_type* _message_ptr { nullptr }; + bool* _new_message_ptr { nullptr }; + + std::string _topic_name; + std::atomic _alive { false }; +}; + +} // namespace ros2 +} // namespace carla diff --git a/LibCarla/source/carla/ros2/middleware/fastdds/GenericCdrPubSubType.h b/LibCarla/source/carla/ros2/middleware/fastdds/GenericCdrPubSubType.h new file mode 100644 index 00000000000..c104580e298 --- /dev/null +++ b/LibCarla/source/carla/ros2/middleware/fastdds/GenericCdrPubSubType.h @@ -0,0 +1,151 @@ +// 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 . + +#pragma once + +#include "carla/ros2/types/CdrSerialization.h" +#include "carla/ros2/types/CdrTopicInfo.h" + +#include +#include +#include +#include + +#include +#include +#include + +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. +/// Type name and max size are provided by CdrTopicInfo. +template +class GenericCdrPubSubType : public eprosima::fastdds::dds::TopicDataType { + public: + using SerializedPayload_t = eprosima::fastrtps::rtps::SerializedPayload_t; + + GenericCdrPubSubType() { + setName(CdrTopicInfo::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( + CdrTopicInfo::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( + 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(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(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(data); + + eprosima::fastcdr::FastBuffer fastbuffer( + reinterpret_cast(payload->data), + static_cast(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 getSerializedSizeProvider(void* data) override { + const MsgType* msg = static_cast(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(new MsgType()); + } + + /// Delete a MsgType previously returned by createData(). + void deleteData(void* data) override { + delete static_cast(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 diff --git a/Ros2Native/LibCarlaRos2Native/CMakeLists.txt b/Ros2Native/LibCarlaRos2Native/CMakeLists.txt index ecbd31ce0f7..23d478a9f8d 100644 --- a/Ros2Native/LibCarlaRos2Native/CMakeLists.txt +++ b/Ros2Native/LibCarlaRos2Native/CMakeLists.txt @@ -18,6 +18,8 @@ set ( file ( GLOB LIBCARLA_ROS2_SOURCES + ${LIBCARLA_SOURCE_PATH}/carla/ros2/middleware/*.cpp + ${LIBCARLA_SOURCE_PATH}/carla/ros2/middleware/fastdds/*.cpp ${LIBCARLA_SOURCE_PATH}/carla/ros2/publishers/*.cpp ${LIBCARLA_SOURCE_PATH}/carla/ros2/subscribers/*.cpp ${LIBCARLA_SOURCE_PATH}/carla/ros2/listeners/*.cpp @@ -27,6 +29,8 @@ file ( file ( GLOB LIBCARLA_ROS2_HEADERS + ${LIBCARLA_SOURCE_PATH}/carla/ros2/middleware/*.h + ${LIBCARLA_SOURCE_PATH}/carla/ros2/middleware/fastdds/*.h ${LIBCARLA_SOURCE_PATH}/carla/ros2/publishers/*.h ${LIBCARLA_SOURCE_PATH}/carla/ros2/subscribers/*.h ${LIBCARLA_SOURCE_PATH}/carla/ros2/listeners/*.h @@ -51,6 +55,8 @@ target_compile_definitions ( carla-ros2-native PUBLIC BOOST_ASIO_ENABLE_BUFFER_DEBUGGING + PRIVATE + CARLA_ROS2_MIDDLEWARE_FASTDDS ) target_link_libraries ( From 26eabf7a46a5587d1ac0ffe44de882b23e97d2ae Mon Sep 17 00:00:00 2001 From: Jesus Armando Anaya Date: Tue, 7 Jul 2026 16:23:18 -0700 Subject: [PATCH 5/7] test(ros2): add middleware abstraction tests Extend the ROS 2 serialization suite with the middleware abstraction cases and rename it to test_ros2_middleware.cpp to match the layer it now covers. 32 new cases, all pure logic with no DDS participant: - middleware_to_string / middleware_from_string / middleware_available / middleware_type_name: the Middleware enum string and type-name helpers, and compile-time availability reporting. - MiddlewareFactoryFixture: set/get, strict resolution, and that creating an unavailable vendor middleware returns nullptr. - generic_cdr_pubsubtype: GenericCdrPubSubType round-trips a fixed-size and a string-bearing message through a real FastDDS SerializedPayload_t, name matches CdrTopicInfo, createData/deleteData and getKey behave. - generic_cdr_pubsubtype_large_payload: getSerializedSizeProvider reports the actual instance size so a full camera frame and a large point cloud serialize past the static max size. The file defines CARLA_ROS2_MIDDLEWARE_FASTDDS and CARLA_ROS2_MIDDLEWARE_TESTING so the factory availability branches are active while the real DDS entities stay compiled out; only GenericCdrPubSubType pulls actual Fast-DDS/Fast-CDR headers. When ENABLE_ROS2 is ON, libcarla_test_server now also links libfastrtps.so (added as an INSTALL_BYPRODUCTS on the fastdds ExternalProject) for the TopicDataType base and SerializedPayload_t, with -rpath-link / -rpath / --disable-new-dtags so its private foonathan_memory dependency resolves at link and run time. When OFF, the renamed source is removed from the glob and the suite builds exactly as before. (adapted from ue4-dev 82c28e28b6b60fcd71ef550fe9b6cd0e678c2794) (adapted from ue4-dev f53144c97f5f1225606b9d4f8be4e35af1c7d5f8) (adapted from ue4-dev c64e8f49d44b35f9e1782caaea546a361600698b) --- LibCarla/CMakeLists.txt | 23 +- ...alization.cpp => test_ros2_middleware.cpp} | 408 +++++++++++++++++- Ros2Native/CMakeLists.txt | 1 + 3 files changed, 427 insertions(+), 5 deletions(-) rename LibCarla/source/test/server/{test_ros2_serialization.cpp => test_ros2_middleware.cpp} (60%) diff --git a/LibCarla/CMakeLists.txt b/LibCarla/CMakeLists.txt index dd38abbd44a..a75941077a1 100644 --- a/LibCarla/CMakeLists.txt +++ b/LibCarla/CMakeLists.txt @@ -467,7 +467,7 @@ if (BUILD_LIBCARLA_TESTS) list ( REMOVE_ITEM LIBCARLA_TEST_SERVER_SOURCES - ${LIBCARLA_SOURCE_PATH}/test/server/test_ros2_serialization.cpp + ${LIBCARLA_SOURCE_PATH}/test/server/test_ros2_middleware.cpp ) endif () @@ -500,9 +500,11 @@ if (BUILD_LIBCARLA_TESTS) ) if (ENABLE_ROS2) - # The CDR serialization tests need the Fast-CDR headers and shared lib - # that the Ros2Native ExternalProject chain installs at build time. The - # path mirrors PROJECT_INSTALL_PATH in Ros2Native/CMakeLists.txt. + # The ROS2 middleware tests need the Fast-CDR and Fast-DDS headers and + # shared libs that the Ros2Native ExternalProject chain installs at build + # time (Fast-DDS for GenericCdrPubSubType's TopicDataType base and + # SerializedPayload_t). The path mirrors PROJECT_INSTALL_PATH in + # Ros2Native/CMakeLists.txt. set (CARLA_ROS2_NATIVE_INSTALL_PATH ${CMAKE_BINARY_DIR}/Ros2Native/install) target_include_directories ( libcarla_test_server SYSTEM PRIVATE @@ -511,6 +513,19 @@ if (BUILD_LIBCARLA_TESTS) target_link_libraries ( libcarla_test_server PRIVATE ${CARLA_ROS2_NATIVE_INSTALL_PATH}/lib/libfastcdr.so + ${CARLA_ROS2_NATIVE_INSTALL_PATH}/lib/libfastrtps.so + ) + # libfastrtps.so privately depends on the foonathan_memory shared lib, + # whose file name is version- and config-dependent (e.g. + # libfoonathan_memory-0.7.4-dbg.so in Debug). -rpath-link lets the linker + # resolve those transitive symbols, and --disable-new-dtags emits DT_RPATH + # (instead of DT_RUNPATH) so the dynamic loader also searches this + # directory for libfastrtps.so's own dependencies at run time. + target_link_options ( + libcarla_test_server PRIVATE + "LINKER:-rpath-link,${CARLA_ROS2_NATIVE_INSTALL_PATH}/lib" + "LINKER:-rpath,${CARLA_ROS2_NATIVE_INSTALL_PATH}/lib" + "LINKER:--disable-new-dtags" ) add_dependencies (libcarla_test_server fastdds) endif () diff --git a/LibCarla/source/test/server/test_ros2_serialization.cpp b/LibCarla/source/test/server/test_ros2_middleware.cpp similarity index 60% rename from LibCarla/source/test/server/test_ros2_serialization.cpp rename to LibCarla/source/test/server/test_ros2_middleware.cpp index 243159e903c..529c8a91443 100644 --- a/LibCarla/source/test/server/test_ros2_serialization.cpp +++ b/LibCarla/source/test/server/test_ros2_middleware.cpp @@ -3,15 +3,24 @@ // For a copy, see . // Must be defined before any includes to suppress real DDS auto-includes -// when the middleware abstraction test groups join this suite. +// (the vendor pub/sub middleware and FastDDSSharedParticipant), while keeping +// the CARLA_ROS2_MIDDLEWARE_FASTDDS compile-time availability branches active. #define CARLA_ROS2_MIDDLEWARE_TESTING +#define CARLA_ROS2_MIDDLEWARE_FASTDDS #include "test.h" +#include +#include +#include +#include +#include #include #include #include +#include +#include #include // ========================================================================== @@ -601,3 +610,400 @@ TEST(cdr_serialization, cdr_serialized_size_pointcloud2_exceeds_static_max) { bytes.data(), bytes.size(), recovered)); EXPECT_EQ(recovered.data.size(), data_bytes); } + +// ========================================================================== +// Middleware abstraction test infrastructure +// ========================================================================== + +using namespace carla::ros2; + +namespace { + +struct TestMsg { + int value{0}; +}; + +struct TestPubTraits { + using msg_type = TestMsg; +}; + +struct TestSubTraits { + using msg_type = TestMsg; +}; + +} // namespace + +// -- Factory fixture (resets static state) ------------------------------------ + +class MiddlewareFactoryFixture : public ::testing::Test { + protected: + void SetUp() override { + MiddlewareFactory::SetMiddleware(Middleware::FastDDS); + } +}; + +// ========================================================================== +// Group 3: middleware_to_string (2 tests) +// ========================================================================== + +TEST(middleware_to_string, fastdds_returns_correct_string) { + EXPECT_STREQ(MiddlewareToString(Middleware::FastDDS), "FastDDS"); +} + +TEST(middleware_to_string, cyclonedds_returns_correct_string) { + EXPECT_STREQ(MiddlewareToString(Middleware::CycloneDDS), "CycloneDDS"); +} + +// ========================================================================== +// Group 4: middleware_from_string (6 tests) +// ========================================================================== + +TEST(middleware_from_string, fastdds_lowercase_valid) { + auto result = MiddlewareFromString("fastdds"); + EXPECT_TRUE(result.valid); + EXPECT_EQ(result.middleware, Middleware::FastDDS); +} + +TEST(middleware_from_string, unknown_string_invalid) { + auto result = MiddlewareFromString("unknowndds"); + EXPECT_FALSE(result.valid); +} + +TEST(middleware_from_string, cyclonedds_lowercase_valid) { + auto result = MiddlewareFromString("cyclonedds"); + EXPECT_TRUE(result.valid); + EXPECT_EQ(result.middleware, Middleware::CycloneDDS); +} + +TEST(middleware_from_string, empty_string_invalid) { + auto result = MiddlewareFromString(""); + EXPECT_FALSE(result.valid); +} + +TEST(middleware_from_string, uppercase_rejected) { + auto result = MiddlewareFromString("FastDDS"); + EXPECT_FALSE(result.valid); +} + +TEST(middleware_from_string, partial_match_rejected) { + auto result = MiddlewareFromString("fast"); + EXPECT_FALSE(result.valid); +} + +// ========================================================================== +// Group 5: middleware_available (3 tests) +// ========================================================================== + +TEST(middleware_available, fastdds_available) { + EXPECT_TRUE( + MiddlewareFactory::IsMiddlewareAvailable(Middleware::FastDDS)); +} + +TEST(middleware_available, available_string_contains_fastdds) { + std::string available = GetAvailableMiddlewareString(); + EXPECT_NE(available.find("FastDDS"), std::string::npos); +} + +TEST(middleware_available, cyclonedds_not_available_without_macro) { + EXPECT_FALSE( + MiddlewareFactory::IsMiddlewareAvailable(Middleware::CycloneDDS)); +} + +// ========================================================================== +// Group 6: middleware_type_name (4 tests) +// ========================================================================== + +TEST(middleware_type_name, bare_name) { + EXPECT_EQ(ToROS2TypeName("Image"), "dds_::Image_"); +} + +TEST(middleware_type_name, fully_qualified) { + EXPECT_EQ( + ToROS2TypeName("sensor_msgs::msg::Image"), + "sensor_msgs::msg::dds_::Image_"); +} + +TEST(middleware_type_name, single_namespace) { + EXPECT_EQ(ToROS2TypeName("msg::Image"), "msg::dds_::Image_"); +} + +TEST(middleware_type_name, empty_string) { + EXPECT_EQ(ToROS2TypeName(""), "dds_::_"); +} + +// ========================================================================== +// Group 7: MiddlewareFactoryFixture (8 tests) +// ========================================================================== + +TEST_F(MiddlewareFactoryFixture, set_and_get_middleware) { + MiddlewareFactory::SetMiddleware(Middleware::FastDDS); + EXPECT_EQ(MiddlewareFactory::GetMiddleware(), Middleware::FastDDS); +} + +TEST_F(MiddlewareFactoryFixture, default_is_fastdds) { + EXPECT_EQ(MiddlewareFactory::GetMiddleware(), Middleware::FastDDS); +} + +TEST_F(MiddlewareFactoryFixture, resolve_available_middleware) { + auto resolution = + MiddlewareFactory::ResolveMiddleware(Middleware::FastDDS); + EXPECT_TRUE(resolution.success); + EXPECT_EQ(resolution.middleware, Middleware::FastDDS); +} + +TEST_F(MiddlewareFactoryFixture, factory_available_string) { + std::string available = MiddlewareFactory::GetAvailableMiddlewareString(); + EXPECT_NE(available.find("FastDDS"), std::string::npos); +} + +TEST_F(MiddlewareFactoryFixture, set_and_get_cyclonedds) { + MiddlewareFactory::SetMiddleware(Middleware::CycloneDDS); + EXPECT_EQ(MiddlewareFactory::GetMiddleware(), Middleware::CycloneDDS); +} + +TEST_F(MiddlewareFactoryFixture, resolve_unavailable_cyclonedds) { + auto resolution = + MiddlewareFactory::ResolveMiddleware(Middleware::CycloneDDS); + EXPECT_FALSE(resolution.success); + EXPECT_EQ(resolution.middleware, Middleware::CycloneDDS); +} + +TEST_F(MiddlewareFactoryFixture, create_publisher_cyclonedds_unavailable) { + MiddlewareFactory::SetMiddleware(Middleware::CycloneDDS); + ::testing::internal::CaptureStderr(); + auto pub = MiddlewareFactory::CreatePublisher(); + ::testing::internal::GetCapturedStderr(); + EXPECT_EQ(pub, nullptr); +} + +TEST_F(MiddlewareFactoryFixture, create_subscriber_cyclonedds_unavailable) { + MiddlewareFactory::SetMiddleware(Middleware::CycloneDDS); + ::testing::internal::CaptureStderr(); + auto sub = MiddlewareFactory::CreateSubscriber(); + ::testing::internal::GetCapturedStderr(); + EXPECT_EQ(sub, nullptr); +} + +// ========================================================================== +// Group 8: generic_cdr_pubsubtype (6 tests) +// Tests for GenericCdrPubSubType — the single FastDDS TopicDataType +// implementation that replaces 30 fastddsgen-generated PubSubType classes. +// ========================================================================== + +TEST(generic_cdr_pubsubtype, type_name_matches_cdr_topic_info) { + // The name set in the GenericCdrPubSubType constructor must equal + // CdrTopicInfo::type_name() — FastDDS uses it for publisher/subscriber matching. + EXPECT_STREQ( + CdrTopicInfo::type_name(), + GenericCdrPubSubType().getName()); + EXPECT_STREQ( + CdrTopicInfo::type_name(), + GenericCdrPubSubType().getName()); + EXPECT_STREQ( + CdrTopicInfo::type_name(), + GenericCdrPubSubType().getName()); + EXPECT_STREQ( + CdrTopicInfo::type_name(), + GenericCdrPubSubType().getName()); + EXPECT_STREQ( + CdrTopicInfo::type_name(), + GenericCdrPubSubType().getName()); +} + +TEST(generic_cdr_pubsubtype, m_typesize_is_positive) { + // FastDDS uses m_typeSize to pre-allocate payload buffers. + // It must be > 0 for every type (min: max_serialized_size + 4 encapsulation bytes). + EXPECT_GT(GenericCdrPubSubType().m_typeSize, 0u); + EXPECT_GT(GenericCdrPubSubType().m_typeSize, 0u); + EXPECT_GT(GenericCdrPubSubType().m_typeSize, 0u); + EXPECT_GT(GenericCdrPubSubType().m_typeSize, 0u); + EXPECT_GT(GenericCdrPubSubType().m_typeSize, 0u); + EXPECT_GT(GenericCdrPubSubType().m_typeSize, 0u); +} + +TEST(generic_cdr_pubsubtype, serialize_deserialize_fixed_size_via_payload) { + // Round-trip a fixed-size type (Clock) through a SerializedPayload_t buffer. + // This exercises the exact code path FastDDS DataWriter/DataReader use. + using SerializedPayload_t = eprosima::fastrtps::rtps::SerializedPayload_t; + + GenericCdrPubSubType pubsub_type; + msg::Clock original{}; + original.clock.sec = 100; + original.clock.nanosec = 500u; + + SerializedPayload_t payload(1024u); + ASSERT_TRUE(pubsub_type.serialize(static_cast(&original), &payload)); + EXPECT_GT(payload.length, 0u); + + msg::Clock recovered{}; + ASSERT_TRUE(pubsub_type.deserialize(&payload, static_cast(&recovered))); + EXPECT_EQ(recovered.clock.sec, 100); + EXPECT_EQ(recovered.clock.nanosec, 500u); +} + +TEST(generic_cdr_pubsubtype, serialize_deserialize_string_type_via_payload) { + // Round-trip a type containing a std::string (Header) through SerializedPayload_t. + using SerializedPayload_t = eprosima::fastrtps::rtps::SerializedPayload_t; + + GenericCdrPubSubType pubsub_type; + msg::Header original{}; + original.stamp.sec = 42; + original.frame_id = "test_frame"; + + SerializedPayload_t payload(4096u); + ASSERT_TRUE(pubsub_type.serialize(static_cast(&original), &payload)); + EXPECT_GT(payload.length, 0u); + + msg::Header recovered{}; + ASSERT_TRUE(pubsub_type.deserialize(&payload, static_cast(&recovered))); + EXPECT_EQ(recovered.stamp.sec, 42); + EXPECT_EQ(recovered.frame_id, "test_frame"); +} + +TEST(generic_cdr_pubsubtype, create_and_delete_data) { + GenericCdrPubSubType pubsub_type; + + void* data = pubsub_type.createData(); + ASSERT_NE(data, nullptr); + + // Cast to verify it is a properly-constructed Clock + msg::Clock* clock = static_cast(data); + EXPECT_EQ(clock->clock.sec, 0); + EXPECT_EQ(clock->clock.nanosec, 0u); + + // Must not crash + pubsub_type.deleteData(data); +} + +TEST(generic_cdr_pubsubtype, getkey_returns_false) { + // CARLA has no keyed topics — getKey must always return false. + GenericCdrPubSubType pubsub_type; + EXPECT_FALSE(pubsub_type.m_isGetKeyDefined); + EXPECT_FALSE(pubsub_type.getKey(nullptr, nullptr, false)); +} + +// ========================================================================== +// Group 9: generic_cdr_pubsubtype_large_payload (3 tests) +// Tests that getSerializedSizeProvider() returns the actual instance size and +// that serialize/deserialize succeed for payloads exceeding max_serialized_size(). +// These tests catch the runtime bug where Image and PointCloud2 publish failed +// with RETCODE_ERROR because the static max size (~648 bytes for Image) was far +// smaller than a real camera frame (~1-8 MB). +// ========================================================================== + +TEST(generic_cdr_pubsubtype_large_payload, image_large_payload_serialize_succeeds) { + using SerializedPayload_t = eprosima::fastrtps::rtps::SerializedPayload_t; + + // 640x480 BGRA image: 1,228,800 bytes of pixel data. + // This is vastly larger than CdrTopicInfo::max_serialized_size() = 648. + constexpr uint32_t width = 640u; + constexpr uint32_t height = 480u; + constexpr uint32_t channels = 4u; + const uint32_t pixel_bytes = width * height * channels; + + msg::Image original{}; + original.header.stamp.sec = 42; + original.header.frame_id = "camera"; + original.height = height; + original.width = width; + original.encoding = "bgra8"; + original.is_bigendian = 0u; + original.step = width * channels; + original.data.assign(pixel_bytes, 0xABu); + + GenericCdrPubSubType pubsub_type; + + // getSerializedSizeProvider() must report the actual instance size, not the + // static max. The returned size must accommodate all pixel data. + auto size_fn = pubsub_type.getSerializedSizeProvider( + static_cast(&original)); + uint32_t reported_size = size_fn(); + EXPECT_GT(reported_size, pixel_bytes) + << "getSerializedSizeProvider must exceed the raw pixel count"; + + SerializedPayload_t payload(reported_size); + ASSERT_TRUE(pubsub_type.serialize(static_cast(&original), &payload)); + EXPECT_GT(payload.length, pixel_bytes); + + msg::Image recovered{}; + ASSERT_TRUE(pubsub_type.deserialize(&payload, static_cast(&recovered))); + EXPECT_EQ(recovered.header.frame_id, "camera"); + EXPECT_EQ(recovered.height, height); + EXPECT_EQ(recovered.width, width); + EXPECT_EQ(recovered.encoding, "bgra8"); + ASSERT_EQ(recovered.data.size(), pixel_bytes); + EXPECT_EQ(recovered.data[0], 0xABu); + EXPECT_EQ(recovered.data[pixel_bytes - 1u], 0xABu); +} + +TEST(generic_cdr_pubsubtype_large_payload, pointcloud2_large_payload_serialize_succeeds) { + using SerializedPayload_t = eprosima::fastrtps::rtps::SerializedPayload_t; + + // 10,000 points x 16 bytes/point = 160,000 bytes of point data. + // This exceeds CdrTopicInfo::max_serialized_size() = 27,597. + constexpr uint32_t num_points = 10000u; + constexpr uint32_t point_step = 16u; + const uint32_t data_bytes = num_points * point_step; + + msg::PointCloud2 original{}; + original.header.frame_id = "velodyne"; + original.height = 1u; + original.width = num_points; + original.point_step = point_step; + original.row_step = num_points * point_step; + original.is_dense = true; + original.data.assign(data_bytes, 0x55u); + + msg::PointField field{}; + field.name = "x"; + field.offset = 0u; + field.datatype = static_cast(msg::PointField::FLOAT32); + field.count = 1u; + original.fields.push_back(field); + + GenericCdrPubSubType pubsub_type; + + auto size_fn = pubsub_type.getSerializedSizeProvider( + static_cast(&original)); + uint32_t reported_size = size_fn(); + EXPECT_GT(reported_size, data_bytes) + << "getSerializedSizeProvider must exceed the raw point data size"; + + SerializedPayload_t payload(reported_size); + ASSERT_TRUE(pubsub_type.serialize(static_cast(&original), &payload)); + EXPECT_GT(payload.length, data_bytes); + + msg::PointCloud2 recovered{}; + ASSERT_TRUE(pubsub_type.deserialize(&payload, static_cast(&recovered))); + EXPECT_EQ(recovered.header.frame_id, "velodyne"); + EXPECT_EQ(recovered.height, 1u); + EXPECT_EQ(recovered.width, num_points); + EXPECT_EQ(recovered.is_dense, true); + ASSERT_EQ(recovered.data.size(), data_bytes); + EXPECT_EQ(recovered.data[0], 0x55u); +} + +TEST(generic_cdr_pubsubtype_large_payload, size_provider_returns_actual_size_not_max) { + // getSerializedSizeProvider() must return different sizes for messages with + // different amounts of variable-length data, proving it is instance-dependent. + msg::Image small_img{}; + small_img.data.assign(100u, 0u); + + msg::Image large_img{}; + large_img.data.assign(100000u, 0u); + + GenericCdrPubSubType pubsub_type; + + auto small_fn = pubsub_type.getSerializedSizeProvider( + static_cast(&small_img)); + auto large_fn = pubsub_type.getSerializedSizeProvider( + static_cast(&large_img)); + + uint32_t small_size = small_fn(); + uint32_t large_size = large_fn(); + + EXPECT_LT(small_size, large_size) + << "Size must grow with the data vector length"; + EXPECT_GT(large_size, 100000u) + << "Size must at least cover the raw data bytes"; +} diff --git a/Ros2Native/CMakeLists.txt b/Ros2Native/CMakeLists.txt index f62c76dabfa..f3ab131c85c 100644 --- a/Ros2Native/CMakeLists.txt +++ b/Ros2Native/CMakeLists.txt @@ -44,6 +44,7 @@ ExternalProject_add ( -DOPENSSL_CRYPTO_LIBRARY:FILEPATH=${UE_OPENSSL_LIBS}/libcrypto.a INSTALL_BYPRODUCTS ${PROJECT_INSTALL_PATH}/lib/libfastcdr.so + ${PROJECT_INSTALL_PATH}/lib/libfastrtps.so DEPENDS foonathan_memory ) From 54c2e98f823afd51ec5bea95c9cc5926d049a830 Mon Sep 17 00:00:00 2001 From: Jesus Armando Anaya Date: Tue, 7 Jul 2026 21:15:23 -0700 Subject: [PATCH 6/7] feat(ros2): route publishers and subscribers through the middleware abstraction Cut the ROS 2 publishers and subscribers over to the middleware strategy layer added earlier in the series. PublisherImpl and SubscriberImpl stop talking to FastDDS directly and delegate to an IPublisherMiddleware / ISubscriberMiddleware created by MiddlewareFactory, so the transport is chosen at runtime with no vendor header reaching these templates. Ported from ue4-dev: - publishers/PublisherImpl.h, subscribers/SubscriberImpl.h: rewritten from raw FastDDS plumbing (participant/writer/reader/listener) to hold a unique_ptr from MiddlewareFactory plus one owned message. The LIBCARLA_WITH_GTEST seams (SetMiddlewareForTesting, and the subscriber's SimulateMessageReceiptForTesting) let the suite inject fakes without a live DDS participant. The traits contract is reduced to a single msg_type typedef. - publishers/Carla*Publisher.cpp, subscribers/*Subscriber.cpp: every publisher and subscriber traits now binds a carla::ros2::msg::* POD instead of the generated sensor_msgs::msg::X + XPubSubType pair, and the message bodies use direct member access in place of the generated field accessors. The DDS type name, hash and size come from the CdrTopicInfo specialization consumed inside the middleware. - ROS2.cpp: ROS2::Enable selects the middleware through the DDS-free SetActiveMiddleware(Middleware::FastDDS) bridge before creating any publisher or subscriber. FastDDS is the only middleware today; the runtime --rmw= selection lands later in the series. The developer BasicPublisher / BasicSubscriber / BasicListener demo path (WITH_ROS2_DEMO) also moves off the generated std_msgs::msg::String: BasicPublisher through its traits, and the raw-FastDDS BasicSubscriber and BasicListener by registering GenericCdrPubSubType, so the generated type files can be removed cleanly in the next PR. FastDDS keeps publishing exactly as before; the wire format is unchanged. (adapted from ue4-dev f2c0760d38cb111cf52c539e645240bfe1701e0f) --- LibCarla/source/carla/ros2/ROS2.cpp | 7 + .../carla/ros2/listeners/BasicListener.cpp | 6 +- .../carla/ros2/publishers/BasicPublisher.cpp | 8 +- .../ros2/publishers/CarlaCameraPublisher.cpp | 69 ++++--- .../ros2/publishers/CarlaClockPublisher.cpp | 10 +- .../publishers/CarlaCollisionPublisher.cpp | 22 +-- .../ros2/publishers/CarlaGNSSPublisher.cpp | 18 +- .../ros2/publishers/CarlaIMUPublisher.cpp | 32 ++-- .../publishers/CarlaPointCloudPublisher.cpp | 66 ++++--- .../publishers/CarlaTransformPublisher.cpp | 37 ++-- .../ros2/publishers/PointCloudFieldsLayout.h | 14 +- .../carla/ros2/publishers/PublisherImpl.h | 159 +++++----------- .../AckermannControlSubscriber.cpp | 16 +- .../ros2/subscribers/BasicSubscriber.cpp | 8 +- .../CarlaEgoVehicleControlSubscriber.cpp | 20 +- .../carla/ros2/subscribers/SubscriberImpl.h | 172 +++++------------- 16 files changed, 247 insertions(+), 417 deletions(-) diff --git a/LibCarla/source/carla/ros2/ROS2.cpp b/LibCarla/source/carla/ros2/ROS2.cpp index 23be8b0bc38..6230b33dc0f 100644 --- a/LibCarla/source/carla/ros2/ROS2.cpp +++ b/LibCarla/source/carla/ros2/ROS2.cpp @@ -16,6 +16,8 @@ #include "carla/sensor/s11n/ImageSerializer.h" #include "carla/sensor/s11n/SensorHeaderSerializer.h" +#include "carla/ros2/middleware/ActiveMiddleware.h" + #include "publishers/BasePublisher.h" #include "publishers/CarlaCameraPublisher.h" #include "publishers/CarlaClockPublisher.h" @@ -90,6 +92,11 @@ enum ESensors { void ROS2::Enable(bool enable) { _enabled = enable; log_info("ROS2 enabled: ", _enabled); + // Select the ROS 2 middleware before any publisher or subscriber is created. + // FastDDS is the only middleware available today; runtime --rmw= selection + // arrives in a later PR. SetActiveMiddleware is the DDS-free bridge that keeps + // vendor headers out of carla-server. + SetActiveMiddleware(Middleware::FastDDS); _clock_publisher = std::make_shared(); #if defined(WITH_ROS2_DEMO) _basic_publisher = std::make_shared(); diff --git a/LibCarla/source/carla/ros2/listeners/BasicListener.cpp b/LibCarla/source/carla/ros2/listeners/BasicListener.cpp index ccb93a8801b..1522d391c2a 100644 --- a/LibCarla/source/carla/ros2/listeners/BasicListener.cpp +++ b/LibCarla/source/carla/ros2/listeners/BasicListener.cpp @@ -5,7 +5,7 @@ #include #include #include -#include "carla/ros2/types/String.h" +#include "carla/ros2/types/msg/String.h" #include "carla/ros2/subscribers/BasicSubscriber.h" #include "carla/ros2/ROS2CallbackData.h" @@ -24,7 +24,7 @@ namespace ros2 { int _matched {0}; BasicSubscriber* _owner {nullptr}; - std_msgs::msg::String _message {}; + msg::String _message {}; }; void BasicListenerImpl::on_subscription_matched(efd::DataReader* reader, const efd::SubscriptionMatchedStatus& info) @@ -47,7 +47,7 @@ namespace ros2 { efd::SampleInfo info; eprosima::fastrtps::types::ReturnCode_t rcode = reader->take_next_sample(&_message, &info); if (rcode == erc::ReturnCodeValue::RETCODE_OK) { - _owner->ForwardMessage(_message.data()); + _owner->ForwardMessage(_message.data); } if (rcode == erc::ReturnCodeValue::RETCODE_ERROR) { std::cerr << "RETCODE_ERROR" << std::endl; diff --git a/LibCarla/source/carla/ros2/publishers/BasicPublisher.cpp b/LibCarla/source/carla/ros2/publishers/BasicPublisher.cpp index 64acde94c04..cd7f0f7902f 100644 --- a/LibCarla/source/carla/ros2/publishers/BasicPublisher.cpp +++ b/LibCarla/source/carla/ros2/publishers/BasicPublisher.cpp @@ -8,8 +8,7 @@ #include "carla/Logging.h" #include "carla/ros2/publishers/PublisherImpl.h" -#include "carla/ros2/types/String.h" -#include "carla/ros2/types/StringPubSubTypes.h" +#include "carla/ros2/types/msg/String.h" namespace carla { namespace ros2 { @@ -19,8 +18,7 @@ constexpr const char *kBasicPublisherTopic = "rt/basic_publisher_example"; } // namespace struct BasicPublisherMsgTraits { - using msg_type = std_msgs::msg::String; - using msg_pubsub_type = std_msgs::msg::StringPubSubType; + using msg_type = msg::String; }; BasicPublisher::BasicPublisher() @@ -38,7 +36,7 @@ bool BasicPublisher::Publish() { } void BasicPublisher::SetData(const std::string &msg) { - _impl->GetMessage()->data(msg); + _impl->GetMessage()->data = msg; } } // namespace ros2 diff --git a/LibCarla/source/carla/ros2/publishers/CarlaCameraPublisher.cpp b/LibCarla/source/carla/ros2/publishers/CarlaCameraPublisher.cpp index b8347158c31..48994d4b390 100644 --- a/LibCarla/source/carla/ros2/publishers/CarlaCameraPublisher.cpp +++ b/LibCarla/source/carla/ros2/publishers/CarlaCameraPublisher.cpp @@ -9,10 +9,8 @@ #include "carla/Logging.h" #include "carla/ros2/publishers/CameraIntrinsics.h" #include "carla/ros2/publishers/PublisherImpl.h" -#include "carla/ros2/types/CameraInfo.h" -#include "carla/ros2/types/CameraInfoPubSubTypes.h" -#include "carla/ros2/types/Image.h" -#include "carla/ros2/types/ImagePubSubTypes.h" +#include "carla/ros2/types/msg/CameraInfo.h" +#include "carla/ros2/types/msg/Image.h" #include #include @@ -21,13 +19,11 @@ namespace carla { namespace ros2 { struct CarlaCameraImageMsgTraits { - using msg_type = sensor_msgs::msg::Image; - using msg_pubsub_type = sensor_msgs::msg::ImagePubSubType; + using msg_type = msg::Image; }; struct CarlaCameraInfoMsgTraits { - using msg_type = sensor_msgs::msg::CameraInfo; - using msg_pubsub_type = sensor_msgs::msg::CameraInfoPubSubType; + using msg_type = msg::CameraInfo; }; CarlaCameraPublisher::CarlaCameraPublisher( @@ -70,27 +66,27 @@ bool CarlaCameraPublisher::WriteCameraInfo( float fov, bool do_rectify) { auto *message = _impl_camera_info->GetMessage(); - message->header().stamp().sec(seconds); - message->header().stamp().nanosec(nanoseconds); - message->header().frame_id(GetFrameId()); + message->header.stamp.sec = seconds; + message->header.stamp.nanosec = nanoseconds; + message->header.frame_id = GetFrameId(); const auto k = ComputeIntrinsics(width, height, fov); - message->height(height); - message->width(width); - message->distortion_model("plumb_bob"); - message->D({0.0, 0.0, 0.0, 0.0, 0.0}); - message->k({k.fx, 0.0, k.cx, 0.0, k.fy, k.cy, 0.0, 0.0, 1.0}); - message->r({1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0}); - message->p({k.fx, 0.0, k.cx, 0.0, 0.0, k.fy, k.cy, 0.0, 0.0, 0.0, 1.0, 0.0}); - message->binning_x(0); - message->binning_y(0); - - message->roi().x_offset(x_offset); - message->roi().y_offset(y_offset); - message->roi().height(height); - message->roi().width(width); - message->roi().do_rectify(do_rectify); + message->height = height; + message->width = width; + message->distortion_model = "plumb_bob"; + message->d = {0.0, 0.0, 0.0, 0.0, 0.0}; + message->k = {k.fx, 0.0, k.cx, 0.0, k.fy, k.cy, 0.0, 0.0, 1.0}; + message->r = {1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0}; + message->p = {k.fx, 0.0, k.cx, 0.0, 0.0, k.fy, k.cy, 0.0, 0.0, 0.0, 1.0, 0.0}; + message->binning_x = 0; + message->binning_y = 0; + + message->roi.x_offset = x_offset; + message->roi.y_offset = y_offset; + message->roi.height = height; + message->roi.width = width; + message->roi.do_rectify = do_rectify; return true; } @@ -111,17 +107,16 @@ bool CarlaCameraPublisher::WriteImage( uint32_t width, std::vector data) { auto *message = _impl_image->GetMessage(); - message->header().stamp().sec(seconds); - message->header().stamp().nanosec(nanoseconds); - message->header().frame_id(GetFrameId()); - - message->width(width); - message->height(height); - message->encoding(GetEncoding()); - message->is_bigendian(0); - message->step(width * GetChannels() * static_cast(sizeof(uint8_t))); - // https://github.com/eProsima/Fast-DDS/issues/2330 - message->data(std::move(data)); + message->header.stamp.sec = seconds; + message->header.stamp.nanosec = nanoseconds; + message->header.frame_id = GetFrameId(); + + message->width = width; + message->height = height; + message->encoding = GetEncoding(); + message->is_bigendian = 0; + message->step = width * GetChannels() * static_cast(sizeof(uint8_t)); + message->data = std::move(data); return true; } diff --git a/LibCarla/source/carla/ros2/publishers/CarlaClockPublisher.cpp b/LibCarla/source/carla/ros2/publishers/CarlaClockPublisher.cpp index 5c47a903d8f..f2e0825cccd 100644 --- a/LibCarla/source/carla/ros2/publishers/CarlaClockPublisher.cpp +++ b/LibCarla/source/carla/ros2/publishers/CarlaClockPublisher.cpp @@ -8,15 +8,13 @@ #include "carla/Logging.h" #include "carla/ros2/publishers/PublisherImpl.h" -#include "carla/ros2/types/Clock.h" -#include "carla/ros2/types/ClockPubSubTypes.h" +#include "carla/ros2/types/msg/Clock.h" namespace carla { namespace ros2 { struct CarlaClockMsgTraits { - using msg_type = rosgraph::msg::Clock; - using msg_pubsub_type = rosgraph::msg::ClockPubSubType; + using msg_type = msg::Clock; }; CarlaClockPublisher::CarlaClockPublisher() @@ -35,8 +33,8 @@ bool CarlaClockPublisher::Publish() { bool CarlaClockPublisher::Write(std::int32_t seconds, std::uint32_t nanoseconds) { auto *message = _impl->GetMessage(); - message->clock().sec(seconds); - message->clock().nanosec(nanoseconds); + message->clock.sec = seconds; + message->clock.nanosec = nanoseconds; return true; } diff --git a/LibCarla/source/carla/ros2/publishers/CarlaCollisionPublisher.cpp b/LibCarla/source/carla/ros2/publishers/CarlaCollisionPublisher.cpp index 9a045a11e4e..0304a84af78 100644 --- a/LibCarla/source/carla/ros2/publishers/CarlaCollisionPublisher.cpp +++ b/LibCarla/source/carla/ros2/publishers/CarlaCollisionPublisher.cpp @@ -8,15 +8,13 @@ #include "carla/Logging.h" #include "carla/ros2/publishers/PublisherImpl.h" -#include "carla/ros2/types/CarlaCollisionEvent.h" -#include "carla/ros2/types/CarlaCollisionEventPubSubTypes.h" +#include "carla/ros2/types/msg/CarlaCollisionEvent.h" namespace carla { namespace ros2 { struct CarlaCollisionMsgTraits { - using msg_type = carla_msgs::msg::CarlaCollisionEvent; - using msg_pubsub_type = carla_msgs::msg::CarlaCollisionEventPubSubType; + using msg_type = msg::CarlaCollisionEvent; }; CarlaCollisionPublisher::CarlaCollisionPublisher( @@ -42,14 +40,14 @@ bool CarlaCollisionPublisher::Write( float impulse_y, float impulse_z) { auto *message = _impl->GetMessage(); - message->header().stamp().sec(seconds); - message->header().stamp().nanosec(nanoseconds); - message->header().frame_id(GetFrameId()); - - message->other_actor_id(other_actor_id); - message->normal_impulse().x(impulse_x); - message->normal_impulse().y(impulse_y); - message->normal_impulse().z(impulse_z); + message->header.stamp.sec = seconds; + message->header.stamp.nanosec = nanoseconds; + message->header.frame_id = GetFrameId(); + + message->other_actor_id = other_actor_id; + message->normal_impulse.x = impulse_x; + message->normal_impulse.y = impulse_y; + message->normal_impulse.z = impulse_z; return true; } diff --git a/LibCarla/source/carla/ros2/publishers/CarlaGNSSPublisher.cpp b/LibCarla/source/carla/ros2/publishers/CarlaGNSSPublisher.cpp index 12784e6deb5..ea3cab36ce9 100644 --- a/LibCarla/source/carla/ros2/publishers/CarlaGNSSPublisher.cpp +++ b/LibCarla/source/carla/ros2/publishers/CarlaGNSSPublisher.cpp @@ -8,15 +8,13 @@ #include "carla/Logging.h" #include "carla/ros2/publishers/PublisherImpl.h" -#include "carla/ros2/types/NavSatFix.h" -#include "carla/ros2/types/NavSatFixPubSubTypes.h" +#include "carla/ros2/types/msg/NavSatFix.h" namespace carla { namespace ros2 { struct CarlaGnssMsgTraits { - using msg_type = sensor_msgs::msg::NavSatFix; - using msg_pubsub_type = sensor_msgs::msg::NavSatFixPubSubType; + using msg_type = msg::NavSatFix; }; CarlaGNSSPublisher::CarlaGNSSPublisher( @@ -41,13 +39,13 @@ bool CarlaGNSSPublisher::Write( double longitude, double altitude) { auto *message = _impl->GetMessage(); - message->header().stamp().sec(seconds); - message->header().stamp().nanosec(nanoseconds); - message->header().frame_id(GetFrameId()); + message->header.stamp.sec = seconds; + message->header.stamp.nanosec = nanoseconds; + message->header.frame_id = GetFrameId(); - message->latitude(latitude); - message->longitude(longitude); - message->altitude(altitude); + message->latitude = latitude; + message->longitude = longitude; + message->altitude = altitude; return true; } diff --git a/LibCarla/source/carla/ros2/publishers/CarlaIMUPublisher.cpp b/LibCarla/source/carla/ros2/publishers/CarlaIMUPublisher.cpp index 7347739c6a0..3fbb4b1a17e 100644 --- a/LibCarla/source/carla/ros2/publishers/CarlaIMUPublisher.cpp +++ b/LibCarla/source/carla/ros2/publishers/CarlaIMUPublisher.cpp @@ -9,15 +9,13 @@ #include "carla/Logging.h" #include "carla/ros2/publishers/ImuMath.h" #include "carla/ros2/publishers/PublisherImpl.h" -#include "carla/ros2/types/Imu.h" -#include "carla/ros2/types/ImuPubSubTypes.h" +#include "carla/ros2/types/msg/Imu.h" namespace carla { namespace ros2 { struct CarlaImuMsgTraits { - using msg_type = sensor_msgs::msg::Imu; - using msg_pubsub_type = sensor_msgs::msg::ImuPubSubType; + using msg_type = msg::Imu; }; CarlaIMUPublisher::CarlaIMUPublisher( @@ -42,24 +40,24 @@ bool CarlaIMUPublisher::Write( float gyro_x, float gyro_y, float gyro_z, float compass) { auto *message = _impl->GetMessage(); - message->header().stamp().sec(seconds); - message->header().stamp().nanosec(nanoseconds); - message->header().frame_id(GetFrameId()); + message->header.stamp.sec = seconds; + message->header.stamp.nanosec = nanoseconds; + message->header.frame_id = GetFrameId(); - message->linear_acceleration().x(accel_x); - message->linear_acceleration().y(accel_y); - message->linear_acceleration().z(accel_z); + message->linear_acceleration.x = accel_x; + message->linear_acceleration.y = accel_y; + message->linear_acceleration.z = accel_z; - message->angular_velocity().x(gyro_x); - message->angular_velocity().y(gyro_y); - message->angular_velocity().z(gyro_z); + message->angular_velocity.x = gyro_x; + message->angular_velocity.y = gyro_y; + message->angular_velocity.z = gyro_z; // Yaw-only quaternion from compass heading; math lives in ImuMath.h. const auto q = OrientationFromCompass(compass); - message->orientation().w(q[0]); - message->orientation().x(q[1]); - message->orientation().y(q[2]); - message->orientation().z(q[3]); + message->orientation.w = q[0]; + message->orientation.x = q[1]; + message->orientation.y = q[2]; + message->orientation.z = q[3]; return true; } diff --git a/LibCarla/source/carla/ros2/publishers/CarlaPointCloudPublisher.cpp b/LibCarla/source/carla/ros2/publishers/CarlaPointCloudPublisher.cpp index 41dbb0b3d2b..4e4ca5a794f 100644 --- a/LibCarla/source/carla/ros2/publishers/CarlaPointCloudPublisher.cpp +++ b/LibCarla/source/carla/ros2/publishers/CarlaPointCloudPublisher.cpp @@ -8,9 +8,8 @@ #include "carla/Logging.h" #include "carla/ros2/publishers/PublisherImpl.h" -#include "carla/ros2/types/PointCloud2.h" -#include "carla/ros2/types/PointCloud2PubSubTypes.h" -#include "carla/ros2/types/PointField.h" +#include "carla/ros2/types/msg/PointCloud2.h" +#include "carla/ros2/types/msg/PointField.h" #include #include @@ -20,40 +19,39 @@ namespace carla { namespace ros2 { struct CarlaPointCloudMsgTraits { - using msg_type = sensor_msgs::msg::PointCloud2; - using msg_pubsub_type = sensor_msgs::msg::PointCloud2PubSubType; + using msg_type = msg::PointCloud2; }; namespace { // Translates the FastDDS-free PointFieldDataType enum (defined in -// PointCloudFieldsLayout.h) to the sensor_msgs::msg::PointField__* numeric -// constants the IDL-generated PointField struct expects on the wire. +// PointCloudFieldsLayout.h) to the carla::ros2::msg::PointField::* numeric +// constants the message struct expects on the wire. std::uint8_t ToPointFieldDatatype(PointFieldDataType datatype) noexcept { switch (datatype) { - case PointFieldDataType::Int8: return sensor_msgs::msg::PointField__INT8; - case PointFieldDataType::UInt8: return sensor_msgs::msg::PointField__UINT8; - case PointFieldDataType::Int16: return sensor_msgs::msg::PointField__INT16; - case PointFieldDataType::UInt16: return sensor_msgs::msg::PointField__UINT16; - case PointFieldDataType::Int32: return sensor_msgs::msg::PointField__INT32; - case PointFieldDataType::UInt32: return sensor_msgs::msg::PointField__UINT32; - case PointFieldDataType::Float32: return sensor_msgs::msg::PointField__FLOAT32; - case PointFieldDataType::Float64: return sensor_msgs::msg::PointField__FLOAT64; + case PointFieldDataType::Int8: return msg::PointField::INT8; + case PointFieldDataType::UInt8: return msg::PointField::UINT8; + case PointFieldDataType::Int16: return msg::PointField::INT16; + case PointFieldDataType::UInt16: return msg::PointField::UINT16; + case PointFieldDataType::Int32: return msg::PointField::INT32; + case PointFieldDataType::UInt32: return msg::PointField::UINT32; + case PointFieldDataType::Float32: return msg::PointField::FLOAT32; + case PointFieldDataType::Float64: return msg::PointField::FLOAT64; } - return sensor_msgs::msg::PointField__FLOAT32; + return msg::PointField::FLOAT32; } -std::vector BuildPointFields( +std::vector BuildPointFields( const PointFieldDescriptor *descriptors, std::size_t count) { - std::vector fields; + std::vector fields; fields.reserve(count); for (std::size_t i = 0; i < count; ++i) { const auto &descriptor = descriptors[i]; - sensor_msgs::msg::PointField field; - field.name(std::string{descriptor.name}); - field.offset(descriptor.offset); - field.datatype(ToPointFieldDatatype(descriptor.datatype)); - field.count(descriptor.count); + msg::PointField field{}; + field.name = std::string{descriptor.name}; + field.offset = descriptor.offset; + field.datatype = ToPointFieldDatatype(descriptor.datatype); + field.count = descriptor.count; fields.push_back(std::move(field)); } return fields; @@ -94,23 +92,23 @@ bool CarlaPointCloudPublisher::WritePointCloud( std::uint32_t width, std::vector data) { auto *message = _impl->GetMessage(); - message->header().stamp().sec(seconds); - message->header().stamp().nanosec(nanoseconds); - message->header().frame_id(GetFrameId()); + message->header.stamp.sec = seconds; + message->header.stamp.nanosec = nanoseconds; + message->header.frame_id = GetFrameId(); const std::size_t point_size = GetPointSize(); - message->width(width); - message->height(height); - message->is_bigendian(false); - message->fields(BuildPointFields(GetFieldDescriptors(), GetFieldDescriptorCount())); - message->point_step(static_cast(point_size)); - message->row_step(static_cast(width * point_size)); + message->width = width; + message->height = height; + message->is_bigendian = false; + message->fields = BuildPointFields(GetFieldDescriptors(), GetFieldDescriptorCount()); + message->point_step = static_cast(point_size); + message->row_step = static_cast(width * point_size); // is_dense=false matches the upstream convention: lidar / radar scans contain // invalid points (no return / max-range hits) and subscribers must not // assume tightly packed valid data. - message->is_dense(false); - message->data(std::move(data)); + message->is_dense = false; + message->data = std::move(data); return true; } diff --git a/LibCarla/source/carla/ros2/publishers/CarlaTransformPublisher.cpp b/LibCarla/source/carla/ros2/publishers/CarlaTransformPublisher.cpp index 25b63dc84da..34c24a2e514 100644 --- a/LibCarla/source/carla/ros2/publishers/CarlaTransformPublisher.cpp +++ b/LibCarla/source/carla/ros2/publishers/CarlaTransformPublisher.cpp @@ -9,8 +9,8 @@ #include "carla/Logging.h" #include "carla/ros2/publishers/PublisherImpl.h" #include "carla/ros2/publishers/TransformQuaternion.h" -#include "carla/ros2/types/TFMessage.h" -#include "carla/ros2/types/TFMessagePubSubTypes.h" +#include "carla/ros2/types/msg/TFMessage.h" +#include "carla/ros2/types/msg/TransformStamped.h" #include @@ -18,8 +18,7 @@ namespace carla { namespace ros2 { struct CarlaTransformMsgTraits { - using msg_type = tf2_msgs::msg::TFMessage; - using msg_pubsub_type = tf2_msgs::msg::TFMessagePubSubType; + using msg_type = msg::TFMessage; }; namespace { @@ -82,21 +81,21 @@ bool CarlaTransformPublisher::Write( tx, ty, tz, pitch_deg, yaw_deg, roll_deg, translation, rotation}); } - geometry_msgs::msg::TransformStamped ts; - ts.header().stamp().sec(seconds); - ts.header().stamp().nanosec(nanoseconds); - ts.header().frame_id(parent_frame_id); - ts.child_frame_id(child_frame_id); - - ts.transform().translation().x(translation[0]); - ts.transform().translation().y(translation[1]); - ts.transform().translation().z(translation[2]); - ts.transform().rotation().w(rotation[0]); - ts.transform().rotation().x(rotation[1]); - ts.transform().rotation().y(rotation[2]); - ts.transform().rotation().z(rotation[3]); - - _impl->GetMessage()->transforms({ts}); + msg::TransformStamped ts{}; + ts.header.stamp.sec = seconds; + ts.header.stamp.nanosec = nanoseconds; + ts.header.frame_id = parent_frame_id; + ts.child_frame_id = child_frame_id; + + ts.transform.translation.x = translation[0]; + ts.transform.translation.y = translation[1]; + ts.transform.translation.z = translation[2]; + ts.transform.rotation.w = rotation[0]; + ts.transform.rotation.x = rotation[1]; + ts.transform.rotation.y = rotation[2]; + ts.transform.rotation.z = rotation[3]; + + _impl->GetMessage()->transforms = {ts}; return true; } diff --git a/LibCarla/source/carla/ros2/publishers/PointCloudFieldsLayout.h b/LibCarla/source/carla/ros2/publishers/PointCloudFieldsLayout.h index ab010ae51d0..b8ba1c38c49 100644 --- a/LibCarla/source/carla/ros2/publishers/PointCloudFieldsLayout.h +++ b/LibCarla/source/carla/ros2/publishers/PointCloudFieldsLayout.h @@ -13,14 +13,12 @@ namespace carla { namespace ros2 { -// PointField datatype constants mirror the IDL-generated constants in -// carla/ros2/types/PointField.h (sensor_msgs::msg::PointField__*). They are -// kept here as an enum class so this header does not pull the FastDDS-generated -// PointField types into the Build-Tests/ compile unit (where FastDDS is not -// on the include path). Producers in publishers/Carla*Publisher.cpp pass the -// values into sensor_msgs::msg::PointField directly; this enum mirrors the -// same numeric values so layout tests can verify field offsets / sizes -// without instantiating FastDDS. +// PointField datatype constants mirror the numeric constants in +// carla/ros2/types/msg/PointField.h (carla::ros2::msg::PointField::*). They are +// kept here as an enum class so this header does not pull the message types into +// the Build-Tests/ compile unit. Producers in publishers/Carla*Publisher.cpp map +// these values onto carla::ros2::msg::PointField; this enum mirrors the same +// numeric values so layout tests can verify field offsets / sizes. enum class PointFieldDataType : std::uint8_t { Int8 = 1, UInt8 = 2, diff --git a/LibCarla/source/carla/ros2/publishers/PublisherImpl.h b/LibCarla/source/carla/ros2/publishers/PublisherImpl.h index 3110d380515..8759d3d2958 100644 --- a/LibCarla/source/carla/ros2/publishers/PublisherImpl.h +++ b/LibCarla/source/carla/ros2/publishers/PublisherImpl.h @@ -6,150 +6,77 @@ #pragma once -#include #include #include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include - #include "carla/Logging.h" -#include "carla/ros2/FastDDSAliases.h" +#include "carla/ros2/middleware/MiddlewareFactory.h" namespace carla { namespace ros2 { -// PublisherImpl wraps the per-message-type FastDDS plumbing (DomainParticipant, -// Publisher, Topic, DataWriter, TypeSupport) into a single template parameterised by -// a Traits struct that exposes `msg_type` and `msg_pubsub_type` typedefs. Concrete -// publishers (CarlaIMUPublisher, future CarlaCameraPublisher, etc.) hold a -// std::shared_ptr> rather than re-implementing the boilerplate -// each time. The class inherits FastDDS's DataWriterListener directly so the -// publication-matched callback updates the alive flag. +// PublisherImpl owns the transport-neutral publish path for a single message +// type. It delegates all DDS plumbing to an IPublisherMiddleware obtained from +// MiddlewareFactory::CreatePublisher(), so the concrete middleware +// (FastDDS, CycloneDDS, ...) is selected at runtime without any vendor header +// reaching this template. The Traits struct only has to expose a `msg_type` +// typedef naming a carla::ros2::msg::* POD; the CDR type name/hash/size come +// from the CdrTopicInfo specialisation consumed inside the middleware. +// Concrete publishers hold a std::shared_ptr>, populate +// the owned message in place via GetMessage(), then call Publish(). template -class PublisherImpl : public efd::DataWriterListener { +class PublisherImpl { public: using msg_type = typename Traits::msg_type; - using msg_pubsub_type = typename Traits::msg_pubsub_type; - - void on_publication_matched( - efd::DataWriter * /*writer*/, - const efd::PublicationMatchedStatus &info) override { - _alive.store(info.current_count > 0, std::memory_order_release); - } - - ~PublisherImpl() override { - if (_datawriter) - _publisher->delete_datawriter(_datawriter); - - if (_publisher) - _participant->delete_publisher(_publisher); - - if (_topic) - _participant->delete_topic(_topic); - - if (_participant) - efd::DomainParticipantFactory::get_instance()->delete_participant(_participant); - } bool Init(std::string topic_name) { - if (_type == nullptr) { - log_error("PublisherImpl::Init invalid TypeSupport"); - return false; - } - - efd::DomainParticipantQos pqos = efd::PARTICIPANT_QOS_DEFAULT; - auto factory = efd::DomainParticipantFactory::get_instance(); - _participant = factory->create_participant(0, pqos); - if (_participant == nullptr) { - log_error("PublisherImpl::Init failed to create DomainParticipant"); - return false; - } - - erc type_rcode = _type.register_type(_participant); - if (type_rcode != erc::ReturnCodeValue::RETCODE_OK) { - log_error("PublisherImpl::Init failed to register type with code:", type_rcode()); - return false; - } - - efd::PublisherQos pubqos = efd::PUBLISHER_QOS_DEFAULT; - _publisher = _participant->create_publisher(pubqos, nullptr); - if (_publisher == nullptr) { - log_error("PublisherImpl::Init failed to create Publisher"); - return false; +#ifdef LIBCARLA_WITH_GTEST + // A test may inject a fake middleware before Init(); do not overwrite it. + if (!_middleware) { +#endif + _middleware = MiddlewareFactory::CreatePublisher(); + if (!_middleware) { + log_error("PublisherImpl::Init failed to create middleware publisher"); + return false; + } +#ifdef LIBCARLA_WITH_GTEST } +#endif + return _middleware->Init(topic_name); + } - efd::TopicQos tqos = efd::TOPIC_QOS_DEFAULT; - _topic = _participant->create_topic(topic_name, _type->getName(), tqos); - if (_topic == nullptr) { - log_error("PublisherImpl::Init failed to create Topic"); - return false; + std::string GetTopicName() { + if (_middleware) { + return _middleware->GetTopicName(); } + return ""; + } - efd::DataWriterQos wqos = efd::DATAWRITER_QOS_DEFAULT; - wqos.endpoint().history_memory_policy = - eprosima::fastrtps::rtps::PREALLOCATED_WITH_REALLOC_MEMORY_MODE; - efd::DataWriterListener *listener = static_cast(this); - _datawriter = _publisher->create_datawriter(_topic, wqos, listener); - if (_datawriter == nullptr) { - log_error("PublisherImpl::Init failed to create DataWriter"); - return false; + bool IsAlive() { + if (_middleware) { + return _middleware->IsAlive(); } - - _topic_name = std::move(topic_name); - return true; + return false; } - [[nodiscard]] const std::string &GetTopicName() const noexcept { return _topic_name; } - - [[nodiscard]] bool IsAlive() const noexcept { return _alive.load(std::memory_order_acquire); } - msg_type *GetMessage() { return &_message; } bool Publish() { - if (_datawriter == nullptr) { - // A failed Init() leaves the publisher cached, so Publish() is invoked - // once per frame for the lifetime of the sensor. Log this only once to - // avoid flooding the server log every frame. - if (!_init_error_logged) { - log_error( - "PublisherImpl::Publish (", _topic_name, - ") called before successful Init(); suppressing further messages for this publisher."); - _init_error_logged = true; - } + if (!_middleware) { + log_error("PublisherImpl::Publish called before Init"); return false; } - eprosima::fastrtps::rtps::InstanceHandle_t instance_handle; - erc rcode = _datawriter->write(&_message, instance_handle); - if (rcode == erc::ReturnCodeValue::RETCODE_OK) { - return true; - } - log_error("PublisherImpl::Publish (", _topic_name, ") failed with code:", rcode()); - return false; + return _middleware->Publish(&_message); } -private: - efd::DomainParticipant *_participant{nullptr}; - efd::Publisher *_publisher{nullptr}; - efd::Topic *_topic{nullptr}; - efd::DataWriter *_datawriter{nullptr}; - efd::TypeSupport _type{new msg_pubsub_type()}; +#ifdef LIBCARLA_WITH_GTEST + void SetMiddlewareForTesting(std::unique_ptr middleware) { + _middleware = std::move(middleware); + } +#endif - std::string _topic_name; - std::atomic _alive{false}; - bool _init_error_logged{false}; +private: + std::unique_ptr _middleware; msg_type _message{}; }; diff --git a/LibCarla/source/carla/ros2/subscribers/AckermannControlSubscriber.cpp b/LibCarla/source/carla/ros2/subscribers/AckermannControlSubscriber.cpp index 7cad00e87f2..2eeb5b78490 100644 --- a/LibCarla/source/carla/ros2/subscribers/AckermannControlSubscriber.cpp +++ b/LibCarla/source/carla/ros2/subscribers/AckermannControlSubscriber.cpp @@ -10,15 +10,13 @@ #include "carla/ros2/ROS2CallbackData.h" #include "carla/ros2/subscribers/AckermannControlConversion.h" #include "carla/ros2/subscribers/SubscriberImpl.h" -#include "carla/ros2/types/AckermannDriveStamped.h" -#include "carla/ros2/types/AckermannDriveStampedPubSubTypes.h" +#include "carla/ros2/types/msg/AckermannDriveStamped.h" namespace carla { namespace ros2 { struct AckermannControlTraits { - using msg_type = ackermann_msgs::msg::AckermannDriveStamped; - using msg_pubsub_type = ackermann_msgs::msg::AckermannDriveStampedPubSubType; + using msg_type = msg::AckermannDriveStamped; }; AckermannControlSubscriber::AckermannControlSubscriber( @@ -36,11 +34,11 @@ AckermannControlSubscriber::~AckermannControlSubscriber() = default; ROS2CallbackData AckermannControlSubscriber::GetMessage() { auto message = _impl->GetMessage(); return FromAckermannDrive( - message.drive().steering_angle(), - message.drive().steering_angle_velocity(), - message.drive().speed(), - message.drive().acceleration(), - message.drive().jerk()); + message.drive.steering_angle, + message.drive.steering_angle_velocity, + message.drive.speed, + message.drive.acceleration, + message.drive.jerk); } void AckermannControlSubscriber::ProcessMessages(ActorCallback callback) { diff --git a/LibCarla/source/carla/ros2/subscribers/BasicSubscriber.cpp b/LibCarla/source/carla/ros2/subscribers/BasicSubscriber.cpp index 9bbe83c0a01..1a28ac32093 100644 --- a/LibCarla/source/carla/ros2/subscribers/BasicSubscriber.cpp +++ b/LibCarla/source/carla/ros2/subscribers/BasicSubscriber.cpp @@ -1,7 +1,7 @@ #include "BasicSubscriber.h" -#include "carla/ros2/types/String.h" -#include "carla/ros2/types/StringPubSubTypes.h" +#include "carla/ros2/types/msg/String.h" +#include "carla/ros2/middleware/fastdds/GenericCdrPubSubType.h" #include "carla/ros2/listeners/BasicListener.h" #include @@ -33,9 +33,9 @@ namespace ros2 { efd::Subscriber* _subscriber { nullptr }; efd::Topic* _topic { nullptr }; efd::DataReader* _datareader { nullptr }; - efd::TypeSupport _type { new std_msgs::msg::StringPubSubType() }; + efd::TypeSupport _type { new GenericCdrPubSubType() }; BasicListener _listener {nullptr}; - std_msgs::msg::String _event {}; + msg::String _event {}; std::string _message {}; bool _new_message {false}; bool _alive {true}; diff --git a/LibCarla/source/carla/ros2/subscribers/CarlaEgoVehicleControlSubscriber.cpp b/LibCarla/source/carla/ros2/subscribers/CarlaEgoVehicleControlSubscriber.cpp index be88f14a9b2..65e8a954158 100644 --- a/LibCarla/source/carla/ros2/subscribers/CarlaEgoVehicleControlSubscriber.cpp +++ b/LibCarla/source/carla/ros2/subscribers/CarlaEgoVehicleControlSubscriber.cpp @@ -9,15 +9,13 @@ #include "carla/Logging.h" #include "carla/ros2/ROS2CallbackData.h" #include "carla/ros2/subscribers/SubscriberImpl.h" -#include "carla/ros2/types/CarlaEgoVehicleControl.h" -#include "carla/ros2/types/CarlaEgoVehicleControlPubSubTypes.h" +#include "carla/ros2/types/msg/CarlaEgoVehicleControl.h" namespace carla { namespace ros2 { struct CarlaEgoVehicleControlTraits { - using msg_type = carla_msgs::msg::CarlaEgoVehicleControl; - using msg_pubsub_type = carla_msgs::msg::CarlaEgoVehicleControlPubSubType; + using msg_type = msg::CarlaEgoVehicleControl; }; CarlaEgoVehicleControlSubscriber::CarlaEgoVehicleControlSubscriber( @@ -36,13 +34,13 @@ ROS2CallbackData CarlaEgoVehicleControlSubscriber::GetMessage() { auto message = _impl->GetMessage(); VehicleControl control; - control.throttle = message.throttle(); - control.steer = message.steer(); - control.brake = message.brake(); - control.hand_brake = message.hand_brake(); - control.reverse = message.reverse(); - control.gear = message.gear(); - control.manual_gear_shift = message.manual_gear_shift(); + control.throttle = message.throttle; + control.steer = message.steer; + control.brake = message.brake; + control.hand_brake = message.hand_brake; + control.reverse = message.reverse; + control.gear = message.gear; + control.manual_gear_shift = message.manual_gear_shift; return control; } diff --git a/LibCarla/source/carla/ros2/subscribers/SubscriberImpl.h b/LibCarla/source/carla/ros2/subscribers/SubscriberImpl.h index 165767a5884..cee76a94f5a 100644 --- a/LibCarla/source/carla/ros2/subscribers/SubscriberImpl.h +++ b/LibCarla/source/carla/ros2/subscribers/SubscriberImpl.h @@ -6,160 +6,80 @@ #pragma once -#include #include -#include #include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include - #include "carla/Logging.h" -#include "carla/ros2/FastDDSAliases.h" +#include "carla/ros2/middleware/MiddlewareFactory.h" +#include "carla/ros2/subscribers/BaseSubscriber.h" namespace carla { namespace ros2 { -// SubscriberImpl mirrors PublisherImpl on the subscriber side: it bundles the FastDDS -// DomainParticipant/Subscriber/Topic/DataReader plumbing into a template parameterised -// by a Traits struct that exposes `msg_type` and `msg_pubsub_type` typedefs. The -// class inherits FastDDS's DataReaderListener directly so on_data_available is a -// member; arriving messages flip _new_message true and the next ProcessMessages call -// on the owning BaseSubscriber drains them. +// SubscriberImpl mirrors PublisherImpl on the subscriber side. It delegates all +// DDS plumbing to an ISubscriberMiddleware obtained from +// MiddlewareFactory::CreateSubscriber(); the middleware writes arriving +// samples directly into the SubscriberImpl-owned _message storage and raises the +// _new_message flag passed down in Init(). The Traits struct only has to expose a +// `msg_type` typedef naming a carla::ros2::msg::* POD. The next ProcessMessages +// call on the owning BaseSubscriber drains a fresh sample via GetMessage(). template -class SubscriberImpl : public efd::DataReaderListener { +class SubscriberImpl { public: using msg_type = typename Traits::msg_type; - using msg_pubsub_type = typename Traits::msg_pubsub_type; - - void on_subscription_matched( - efd::DataReader * /*reader*/, - const efd::SubscriptionMatchedStatus &info) override { - _alive.store(info.current_count > 0, std::memory_order_release); - } - - void on_data_available(efd::DataReader *reader) override { - // FastDDS invokes this on a DDS listener thread. Take into a stack-local - // sample first so the lock only spans the copy into _message, and so a - // dispose/unregister notification (RETCODE_OK with !info.valid_data) does - // not flip _new_message. The _new_message store is held inside the lock - // so a concurrent GetMessage cannot race-clear the flag after we publish - // a fresh sample. - efd::SampleInfo info; - msg_type sample{}; - erc rcode = reader->take_next_sample(&sample, &info); - if (rcode != erc::ReturnCodeValue::RETCODE_OK) { - log_error("SubscriberImpl::on_data_available (", _topic_name, ") failed with code:", rcode()); - return; - } - if (!info.valid_data) { - return; - } - std::lock_guard lock(_message_mutex); - _message = std::move(sample); - _new_message.store(true, std::memory_order_release); - } - - ~SubscriberImpl() override { - if (_datareader) - _subscriber->delete_datareader(_datareader); - - if (_subscriber) - _participant->delete_subscriber(_subscriber); - - if (_topic) - _participant->delete_topic(_topic); - - if (_participant) - efd::DomainParticipantFactory::get_instance()->delete_participant(_participant); - } bool Init(std::string topic_name) { - if (_type == nullptr) { - log_error("SubscriberImpl::Init invalid TypeSupport"); - return false; - } - - efd::DomainParticipantQos pqos = efd::PARTICIPANT_QOS_DEFAULT; - auto factory = efd::DomainParticipantFactory::get_instance(); - _participant = factory->create_participant(0, pqos); - if (_participant == nullptr) { - log_error("SubscriberImpl::Init failed to create DomainParticipant"); - return false; - } - - erc type_rcode = _type.register_type(_participant); - if (type_rcode != erc::ReturnCodeValue::RETCODE_OK) { - log_error("SubscriberImpl::Init failed to register type with code:", type_rcode()); - return false; - } - - efd::SubscriberQos subqos = efd::SUBSCRIBER_QOS_DEFAULT; - _subscriber = _participant->create_subscriber(subqos, nullptr); - if (_subscriber == nullptr) { - log_error("SubscriberImpl::Init failed to create Subscriber"); - return false; +#ifdef LIBCARLA_WITH_GTEST + // A test may inject a fake middleware before Init(); do not overwrite it. + if (!_middleware) { +#endif + _middleware = MiddlewareFactory::CreateSubscriber(); + if (!_middleware) { + log_error("SubscriberImpl::Init failed to create middleware subscriber"); + return false; + } +#ifdef LIBCARLA_WITH_GTEST } +#endif + return _middleware->Init(topic_name, &_message, &_new_message); + } - efd::TopicQos tqos = efd::TOPIC_QOS_DEFAULT; - _topic = _participant->create_topic(topic_name, _type->getName(), tqos); - if (_topic == nullptr) { - log_error("SubscriberImpl::Init failed to create Topic"); - return false; + std::string GetTopicName() { + if (_middleware) { + return _middleware->GetTopicName(); } + return ""; + } - efd::DataReaderQos rqos = efd::DATAREADER_QOS_DEFAULT; - efd::DataReaderListener *listener = static_cast(this); - _datareader = _subscriber->create_datareader(_topic, rqos, listener); - if (_datareader == nullptr) { - log_error("SubscriberImpl::Init failed to create DataReader"); - return false; + bool IsAlive() { + if (_middleware) { + return _middleware->IsAlive(); } - - _topic_name = std::move(topic_name); - return true; + return false; } - [[nodiscard]] const std::string &GetTopicName() const noexcept { return _topic_name; } + msg_type GetMessage() { + _new_message = false; + return _message; + } - [[nodiscard]] bool IsAlive() const noexcept { return _alive.load(std::memory_order_acquire); } + bool HasNewMessage() { return _new_message; } - msg_type GetMessage() { - std::lock_guard lock(_message_mutex); - msg_type copy = _message; - _new_message.store(false, std::memory_order_release); - return copy; +#ifdef LIBCARLA_WITH_GTEST + void SetMiddlewareForTesting(std::unique_ptr middleware) { + _middleware = std::move(middleware); } - [[nodiscard]] bool HasNewMessage() const noexcept { - return _new_message.load(std::memory_order_acquire); + void SimulateMessageReceiptForTesting(const msg_type &msg) { + _message = msg; + _new_message = true; } +#endif private: - efd::DomainParticipant *_participant{nullptr}; - efd::Subscriber *_subscriber{nullptr}; - efd::Topic *_topic{nullptr}; - efd::DataReader *_datareader{nullptr}; - efd::TypeSupport _type{new msg_pubsub_type()}; - - std::string _topic_name; - std::atomic _alive{false}; - std::atomic _new_message{false}; - std::mutex _message_mutex; + std::unique_ptr _middleware; msg_type _message{}; + bool _new_message{false}; }; } // namespace ros2 From 3e75dacb185fd522c2b5797488995ba94d857018 Mon Sep 17 00:00:00 2001 From: Jesus Armando Anaya Date: Tue, 7 Jul 2026 21:15:23 -0700 Subject: [PATCH 7/7] test(ros2): add publisher and subscriber impl tests Add the PublisherImpl and SubscriberImpl cases to the ROS 2 middleware suite, 14 new cases covering the cutover. Ported from ue4-dev: - MockPublisherMiddleware / MockSubscriberMiddleware: minimal IPublisherMiddleware / ISubscriberMiddleware fakes that record the calls the impls make. - publisher_impl (7): GetMessage returns the owned buffer, Init and Publish delegate to the injected middleware, Publish before Init fails, IsAlive and GetTopicName delegate, and the owned message pointer flows through Publish unchanged. - subscriber_impl (7): the new-message flag starts false, Init threads the storage and flag pointers to the middleware and propagates its result, GetMessage clears the flag, and SimulateMessageReceipt drives the receive path without a live participant. The impls are injected through the LIBCARLA_WITH_GTEST seams, so no DDS participant is created. The cases reuse the TestMsg traits already defined for the middleware groups. (adapted from ue4-dev f2c0760d38cb111cf52c539e645240bfe1701e0f) --- .../test/server/test_ros2_middleware.cpp | 223 ++++++++++++++++++ 1 file changed, 223 insertions(+) diff --git a/LibCarla/source/test/server/test_ros2_middleware.cpp b/LibCarla/source/test/server/test_ros2_middleware.cpp index 529c8a91443..29b87a4780f 100644 --- a/LibCarla/source/test/server/test_ros2_middleware.cpp +++ b/LibCarla/source/test/server/test_ros2_middleware.cpp @@ -14,6 +14,8 @@ #include #include #include +#include +#include #include #include #include @@ -1007,3 +1009,224 @@ TEST(generic_cdr_pubsubtype_large_payload, size_provider_returns_actual_size_not EXPECT_GT(large_size, 100000u) << "Size must at least cover the raw data bytes"; } + +// ========================================================================== +// PublisherImpl / SubscriberImpl test infrastructure +// ========================================================================== +// Reuses the TestMsg / TestPubTraits / TestSubTraits helpers defined above for +// the middleware abstraction groups. + +// -- Mock publisher middleware ------------------------------------------------ + +class MockPublisherMiddleware : public IPublisherMiddleware { +public: + bool init_return_value{true}; + bool publish_return_value{true}; + bool alive{true}; + + bool init_called{false}; + bool publish_called{false}; + std::string last_topic_name; + void *last_published_data{nullptr}; + + bool Init(const std::string &topic_name) override { + init_called = true; + last_topic_name = topic_name; + return init_return_value; + } + + bool Publish(void *message_data) override { + publish_called = true; + last_published_data = message_data; + return publish_return_value; + } + + bool IsAlive() const override { return alive; } + std::string GetTopicName() const override { return last_topic_name; } +}; + +// -- Mock subscriber middleware ----------------------------------------------- + +class MockSubscriberMiddleware : public ISubscriberMiddleware { +public: + bool init_return_value{true}; + bool alive{true}; + + bool init_called{false}; + std::string last_topic_name; + void *stored_message_ptr{nullptr}; + bool *stored_flag_ptr{nullptr}; + + bool Init( + const std::string &topic_name, + void *message_ptr, + bool *new_message_flag) override { + init_called = true; + last_topic_name = topic_name; + stored_message_ptr = message_ptr; + stored_flag_ptr = new_message_flag; + return init_return_value; + } + + bool IsAlive() const override { return alive; } + std::string GetTopicName() const override { return last_topic_name; } +}; + +// ========================================================================== +// Group 10: publisher_impl (7 tests) +// ========================================================================== + +TEST(publisher_impl, get_message_returns_pointer) { + PublisherImpl pub; + TestMsg *msg = pub.GetMessage(); + ASSERT_NE(msg, nullptr); + msg->value = 7; + EXPECT_EQ(pub.GetMessage()->value, 7); +} + +TEST(publisher_impl, init_delegates_to_middleware) { + PublisherImpl pub; + auto *mock = new MockPublisherMiddleware(); + pub.SetMiddlewareForTesting( + std::unique_ptr(mock)); + EXPECT_TRUE(pub.Init("rt/test_topic")); + EXPECT_TRUE(mock->init_called); + EXPECT_EQ(mock->last_topic_name, "rt/test_topic"); +} + +TEST(publisher_impl, publish_delegates_to_middleware) { + PublisherImpl pub; + auto *mock = new MockPublisherMiddleware(); + pub.SetMiddlewareForTesting( + std::unique_ptr(mock)); + pub.Init("rt/test_topic"); + EXPECT_TRUE(pub.Publish()); + EXPECT_TRUE(mock->publish_called); + EXPECT_EQ(mock->last_published_data, pub.GetMessage()); +} + +TEST(publisher_impl, publish_before_init_fails) { + PublisherImpl pub; + ::testing::internal::CaptureStderr(); + EXPECT_FALSE(pub.Publish()); + ::testing::internal::GetCapturedStderr(); +} + +TEST(publisher_impl, is_alive_delegates) { + PublisherImpl pub; + auto *mock = new MockPublisherMiddleware(); + mock->alive = true; + pub.SetMiddlewareForTesting( + std::unique_ptr(mock)); + pub.Init("rt/test_topic"); + EXPECT_TRUE(pub.IsAlive()); + mock->alive = false; + EXPECT_FALSE(pub.IsAlive()); +} + +TEST(publisher_impl, topic_name_delegates) { + PublisherImpl pub; + auto *mock = new MockPublisherMiddleware(); + pub.SetMiddlewareForTesting( + std::unique_ptr(mock)); + pub.Init("rt/camera/image"); + EXPECT_EQ(pub.GetTopicName(), "rt/camera/image"); +} + +TEST(publisher_impl, data_flows_through_publish) { + PublisherImpl pub; + auto *mock = new MockPublisherMiddleware(); + pub.SetMiddlewareForTesting( + std::unique_ptr(mock)); + pub.Init("rt/test_topic"); + + pub.GetMessage()->value = 42; + pub.Publish(); + + ASSERT_NE(mock->last_published_data, nullptr); + auto *published = static_cast(mock->last_published_data); + EXPECT_EQ(published->value, 42); +} + +// ========================================================================== +// Group 11: subscriber_impl (7 tests) +// ========================================================================== + +TEST(subscriber_impl, has_new_message_initially_false) { + SubscriberImpl sub; + EXPECT_FALSE(sub.HasNewMessage()); +} + +TEST(subscriber_impl, init_delegates_to_middleware) { + SubscriberImpl sub; + auto *mock = new MockSubscriberMiddleware(); + sub.SetMiddlewareForTesting( + std::unique_ptr(mock)); + EXPECT_TRUE(sub.Init("rt/test_topic")); + EXPECT_TRUE(mock->init_called); + EXPECT_EQ(mock->last_topic_name, "rt/test_topic"); + EXPECT_NE(mock->stored_message_ptr, nullptr); + EXPECT_NE(mock->stored_flag_ptr, nullptr); +} + +TEST(subscriber_impl, get_message_clears_flag) { + SubscriberImpl sub; + auto *mock = new MockSubscriberMiddleware(); + sub.SetMiddlewareForTesting( + std::unique_ptr(mock)); + sub.Init("rt/test_topic"); + + TestMsg msg; + msg.value = 77; + sub.SimulateMessageReceiptForTesting(msg); + EXPECT_TRUE(sub.HasNewMessage()); + + TestMsg retrieved = sub.GetMessage(); + EXPECT_EQ(retrieved.value, 77); + EXPECT_FALSE(sub.HasNewMessage()); +} + +TEST(subscriber_impl, is_alive_delegates) { + SubscriberImpl sub; + auto *mock = new MockSubscriberMiddleware(); + mock->alive = true; + sub.SetMiddlewareForTesting( + std::unique_ptr(mock)); + sub.Init("rt/test_topic"); + EXPECT_TRUE(sub.IsAlive()); + mock->alive = false; + EXPECT_FALSE(sub.IsAlive()); +} + +TEST(subscriber_impl, topic_name_delegates) { + SubscriberImpl sub; + auto *mock = new MockSubscriberMiddleware(); + sub.SetMiddlewareForTesting( + std::unique_ptr(mock)); + sub.Init("rt/lidar/points"); + EXPECT_EQ(sub.GetTopicName(), "rt/lidar/points"); +} + +TEST(subscriber_impl, simulate_message_receipt) { + SubscriberImpl sub; + auto *mock = new MockSubscriberMiddleware(); + sub.SetMiddlewareForTesting( + std::unique_ptr(mock)); + sub.Init("rt/test_topic"); + + EXPECT_FALSE(sub.HasNewMessage()); + TestMsg msg; + msg.value = 123; + sub.SimulateMessageReceiptForTesting(msg); + EXPECT_TRUE(sub.HasNewMessage()); + EXPECT_EQ(sub.GetMessage().value, 123); +} + +TEST(subscriber_impl, init_failure_propagated) { + SubscriberImpl sub; + auto *mock = new MockSubscriberMiddleware(); + mock->init_return_value = false; + sub.SetMiddlewareForTesting( + std::unique_ptr(mock)); + EXPECT_FALSE(sub.Init("rt/test_topic")); +}