From 0ab35ee687b5c3f8d2d7972577867230bf679813 Mon Sep 17 00:00:00 2001 From: Jesus Armando Anaya Date: Fri, 27 Mar 2026 01:58:10 -0700 Subject: [PATCH 1/7] feat(LibCarla/ros2): add DDS middleware abstraction layer for multi-backend support MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Introduce a strategy-pattern abstraction that decouples PublisherImpl and SubscriberImpl from the FastDDS API, enabling future integration of additional DDS middleware implementations (e.g. CycloneDDS). New abstraction layer (LibCarla/source/carla/ros2/dds/): - DDSMiddleware enum and string conversion utilities - IDDSPublisherMiddleware / IDDSSubscriberMiddleware pure virtual interfaces - DDSMiddlewareFactory with thread-safe middleware selection and creation - FastDDSPublisherMiddleware / FastDDSSubscriberMiddleware template implementations behind the new interfaces - FastDDSTypeMap identity-mapping type traits (to be replaced with real conversions when POD message types are introduced) Modified files: - PublisherImpl.h: replaced direct FastDDS inheritance and members with delegation to IDDSPublisherMiddleware via the factory (public API unchanged) - SubscriberImpl.h: same pattern for subscriber side - cmake/fast_dds/CMakeLists.txt: added CARLA_ROS2_DDS_FASTDDS compile definition, new header globs, and fixed missing include directories in the debug build target No behavior change — FastDDS remains the only compiled middleware. Concrete publishers and subscribers are unaffected. --- LibCarla/cmake/fast_dds/CMakeLists.txt | 7 + .../source/carla/ros2/dds/DDSMiddleware.h | 56 +++++++ .../carla/ros2/dds/DDSMiddlewareFactory.h | 119 ++++++++++++++ .../carla/ros2/dds/IDDSPublisherMiddleware.h | 37 +++++ .../carla/ros2/dds/IDDSSubscriberMiddleware.h | 38 +++++ .../dds/fastdds/FastDDSPublisherMiddleware.h | 144 +++++++++++++++++ .../dds/fastdds/FastDDSSubscriberMiddleware.h | 151 ++++++++++++++++++ .../carla/ros2/dds/fastdds/FastDDSTypeMap.h | 96 +++++++++++ .../carla/ros2/publishers/PublisherImpl.h | 115 +++---------- .../carla/ros2/subscribers/SubscriberImpl.h | 124 ++------------ 10 files changed, 684 insertions(+), 203 deletions(-) create mode 100644 LibCarla/source/carla/ros2/dds/DDSMiddleware.h create mode 100644 LibCarla/source/carla/ros2/dds/DDSMiddlewareFactory.h create mode 100644 LibCarla/source/carla/ros2/dds/IDDSPublisherMiddleware.h create mode 100644 LibCarla/source/carla/ros2/dds/IDDSSubscriberMiddleware.h create mode 100644 LibCarla/source/carla/ros2/dds/fastdds/FastDDSPublisherMiddleware.h create mode 100644 LibCarla/source/carla/ros2/dds/fastdds/FastDDSSubscriberMiddleware.h create mode 100644 LibCarla/source/carla/ros2/dds/fastdds/FastDDSTypeMap.h diff --git a/LibCarla/cmake/fast_dds/CMakeLists.txt b/LibCarla/cmake/fast_dds/CMakeLists.txt index 366549eefaf..4ff74c21d57 100644 --- a/LibCarla/cmake/fast_dds/CMakeLists.txt +++ b/LibCarla/cmake/fast_dds/CMakeLists.txt @@ -9,6 +9,8 @@ file(GLOB libcarla_carla_fastdds_headers "${libcarla_source_path}/carla/ros2/subscribers/*.h" "${libcarla_source_path}/carla/ros2/listeners/*.h" "${libcarla_source_path}/carla/ros2/types/*.h" + "${libcarla_source_path}/carla/ros2/dds/*.h" + "${libcarla_source_path}/carla/ros2/dds/fastdds/*.h" ) install(FILES ${libcarla_carla_fastdds_headers} DESTINATION include/carla/ros2) @@ -30,6 +32,7 @@ file(GLOB libcarla_fastdds_sources if (LIBCARLA_BUILD_RELEASE) add_library(carla_fastdds STATIC ${libcarla_fastdds_sources}) + target_compile_definitions(carla_fastdds PRIVATE CARLA_ROS2_DDS_FASTDDS) target_compile_options(carla_fastdds PRIVATE -fexceptions) target_include_directories(carla_fastdds SYSTEM PRIVATE @@ -48,11 +51,15 @@ if (LIBCARLA_BUILD_DEBUG) add_library(carla_fastdds_debug STATIC ${libcarla_fastdds_sources}) + target_compile_definitions(carla_fastdds_debug PRIVATE CARLA_ROS2_DDS_FASTDDS) target_compile_options(carla_fastdds_debug PRIVATE -fexceptions) target_include_directories(carla_fastdds_debug SYSTEM PRIVATE "${BOOST_INCLUDE_PATH}" "${RPCLIB_INCLUDE_PATH}") + + target_include_directories(carla_fastdds_debug PRIVATE "${FASTDDS_INCLUDE_PATH}") + target_include_directories(carla_fastdds_debug PRIVATE "${libcarla_source_path}/carla/ros2") install(TARGETS carla_fastdds_debug DESTINATION lib) set_target_properties(carla_fastdds_debug PROPERTIES COMPILE_FLAGS "${CMAKE_CXX_FLAGS_DEBUG}") target_compile_definitions(carla_fastdds_debug PUBLIC -DBOOST_ASIO_ENABLE_BUFFER_DEBUGGING) diff --git a/LibCarla/source/carla/ros2/dds/DDSMiddleware.h b/LibCarla/source/carla/ros2/dds/DDSMiddleware.h new file mode 100644 index 00000000000..35a4d89be47 --- /dev/null +++ b/LibCarla/source/carla/ros2/dds/DDSMiddleware.h @@ -0,0 +1,56 @@ +// 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 { + +/// Enumeration of available DDS middleware implementations. +/// Passed to ROS2::Enable() to select the middleware at startup. +/// Once set, the middleware cannot be changed without restarting. +enum class DDSMiddleware { + FastDDS +}; + +/// Convert a DDSMiddleware enum value to a readable string. +inline const char* DDSMiddlewareToString(DDSMiddleware middleware) { + switch (middleware) { + case DDSMiddleware::FastDDS: + return "FastDDS"; + } + return "Unknown"; +} + +/// Result of parsing a middleware name string. +struct DDSMiddlewareParseResult { + bool valid; + DDSMiddleware middleware; +}; + +/// Parse a middleware name string (lowercase). Returns {true, middleware} on match, +/// {false, FastDDS} for unrecognized values. +inline DDSMiddlewareParseResult DDSMiddlewareFromString(const std::string& name) { + if (name == "fastdds") { + return {true, DDSMiddleware::FastDDS}; + } + return {false, DDSMiddleware::FastDDS}; +} + +/// Return a readable list of middleware implementations compiled into this binary. +inline std::string GetAvailableMiddlewareString() { + std::string result; +#if defined(CARLA_ROS2_DDS_FASTDDS) + result += "FastDDS"; +#endif + if (result.empty()) { + result = "none"; + } + return result; +} + +} // namespace ros2 +} // namespace carla diff --git a/LibCarla/source/carla/ros2/dds/DDSMiddlewareFactory.h b/LibCarla/source/carla/ros2/dds/DDSMiddlewareFactory.h new file mode 100644 index 00000000000..5b7b2adcd9a --- /dev/null +++ b/LibCarla/source/carla/ros2/dds/DDSMiddlewareFactory.h @@ -0,0 +1,119 @@ +// 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/dds/DDSMiddleware.h" +#include "carla/ros2/dds/IDDSPublisherMiddleware.h" +#include "carla/ros2/dds/IDDSSubscriberMiddleware.h" +#include "carla/Logging.h" + +#if defined(CARLA_ROS2_DDS_FASTDDS) +# include "carla/ros2/dds/fastdds/FastDDSPublisherMiddleware.h" +# include "carla/ros2/dds/fastdds/FastDDSSubscriberMiddleware.h" +#endif + +namespace carla { +namespace ros2 { + +/// Factory that creates DDS publisher/subscriber middleware based on the active middleware selection. +/// The middleware is set once at startup via SetMiddleware() before any DDS entities are created. +/// After the first entity is created, changing the middleware has undefined behavior. +class DDSMiddlewareFactory { + public: + /// Select the DDS middleware for all subsequent publisher/subscriber creation. + /// Must be called before any publisher or subscriber is initialized. + static void SetMiddleware(DDSMiddleware middleware) { + GetActiveMiddleware() = middleware; + } + + /// @return The currently selected DDS middleware. + static DDSMiddleware GetMiddleware() { + return GetActiveMiddleware(); + } + + /// Check whether a specific middleware was compiled into this binary. + static bool IsMiddlewareAvailable(DDSMiddleware middleware) { + switch (middleware) { + case DDSMiddleware::FastDDS: +#if defined(CARLA_ROS2_DDS_FASTDDS) + return true; +#else + return false; +#endif + } + return false; + } + + /// Result of middleware resolution — whether resolution succeeded and which middleware to use. + struct MiddlewareResolution { + bool success; + DDSMiddleware middleware; + }; + + /// Resolve the requested middleware strictly — no fallback to other middleware. + /// Returns {true, requested} if available, {false, requested} otherwise. + static MiddlewareResolution ResolveMiddleware(DDSMiddleware 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 DDSMiddleware.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 DDSMiddleware::FastDDS: +#if defined(CARLA_ROS2_DDS_FASTDDS) + return std::unique_ptr( + new FastDDSPublisherMiddleware()); +#else + log_error("DDSMiddlewareFactory: FastDDS 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 DDSMiddleware::FastDDS: +#if defined(CARLA_ROS2_DDS_FASTDDS) + return std::unique_ptr( + new FastDDSSubscriberMiddleware()); +#else + log_error("DDSMiddlewareFactory: FastDDS 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 DDSMiddleware& GetActiveMiddleware() { + static DDSMiddleware active_middleware = DDSMiddleware::FastDDS; + return active_middleware; + } +}; + +} // namespace ros2 +} // namespace carla diff --git a/LibCarla/source/carla/ros2/dds/IDDSPublisherMiddleware.h b/LibCarla/source/carla/ros2/dds/IDDSPublisherMiddleware.h new file mode 100644 index 00000000000..4e757df0c67 --- /dev/null +++ b/LibCarla/source/carla/ros2/dds/IDDSPublisherMiddleware.h @@ -0,0 +1,37 @@ +// 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 { + +/// Type-erased abstract interface for a DDS publisher middleware. +/// Concrete implementations handle all vendor-specific DDS entity creation, +/// type registration, and data writing. +class IDDSPublisherMiddleware { + public: + virtual ~IDDSPublisherMiddleware() = default; + + /// Initialize DDS entities (participant, publisher, topic, writer). + /// @param topic_name Full DDS 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 DDS 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 DDS topic name this publisher is bound to. + virtual std::string GetTopicName() const = 0; +}; + +} // namespace ros2 +} // namespace carla diff --git a/LibCarla/source/carla/ros2/dds/IDDSSubscriberMiddleware.h b/LibCarla/source/carla/ros2/dds/IDDSSubscriberMiddleware.h new file mode 100644 index 00000000000..896bc86295d --- /dev/null +++ b/LibCarla/source/carla/ros2/dds/IDDSSubscriberMiddleware.h @@ -0,0 +1,38 @@ +// 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 { + +/// Type-erased abstract interface for a DDS subscriber middleware. +/// Concrete implementations write received messages directly into the caller-provided +/// storage (message_ptr / new_message_flag) to avoid an extra copy. +class IDDSSubscriberMiddleware { + public: + virtual ~IDDSSubscriberMiddleware() = default; + + /// Initialize DDS entities (participant, subscriber, topic, reader). + /// The middleware writes incoming messages to *message_ptr and sets *new_message_flag = true. + /// @param topic_name Full DDS topic name. + /// @param message_ptr Pointer to the message storage owned by SubscriberImpl. + /// @param new_message_flag Pointer to the new-message flag owned by SubscriberImpl. + /// @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 DDS topic name this subscriber is bound to. + virtual std::string GetTopicName() const = 0; +}; + +} // namespace ros2 +} // namespace carla diff --git a/LibCarla/source/carla/ros2/dds/fastdds/FastDDSPublisherMiddleware.h b/LibCarla/source/carla/ros2/dds/fastdds/FastDDSPublisherMiddleware.h new file mode 100644 index 00000000000..07ffec6bbaf --- /dev/null +++ b/LibCarla/source/carla/ros2/dds/fastdds/FastDDSPublisherMiddleware.h @@ -0,0 +1,144 @@ +// 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/dds/IDDSPublisherMiddleware.h" +#include "carla/ros2/dds/fastdds/FastDDSTypeMap.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 IDDSPublisherMiddleware. +/// Parameterized on a traits type T that provides: +/// T::msg_type — the message type +/// Native FastDDS types are resolved via FastDDSTypeMap. +template +class FastDDSPublisherMiddleware + : public IDDSPublisherMiddleware, + public eprosima::fastdds::dds::DataWriterListener { + public: + using msg_type = typename T::msg_type; + using type_map = FastDDSTypeMap; + using fastdds_type = typename type_map::fastdds_type; + using fastdds_pubsub_type = typename type_map::fastdds_pubsub_type; + + void on_publication_matched( + efd::DataWriter* writer, + const efd::PublicationMatchedStatus& info) override { + _alive = (info.total_count > 0); + } + + ~FastDDSPublisherMiddleware() 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(const std::string& topic_name) override { + if (_type == nullptr) { + log_error("FastDDSPublisherMiddleware: 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("FastDDSPublisherMiddleware: Failed to create DomainParticipant"); + return false; + } + _type.register_type(_participant); + + 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; + } + + 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; + 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 { + auto* msg = static_cast(message_data); + to_fastdds(*msg, _fastdds_msg); + eprosima::fastrtps::rtps::InstanceHandle_t instance_handle; + erc rcode = _datawriter->write(&_fastdds_msg, 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; + } + + 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 fastdds_pubsub_type() }; + + fastdds_type _fastdds_msg; + std::string _topic_name; + bool _alive { false }; +}; + +} // namespace ros2 +} // namespace carla diff --git a/LibCarla/source/carla/ros2/dds/fastdds/FastDDSSubscriberMiddleware.h b/LibCarla/source/carla/ros2/dds/fastdds/FastDDSSubscriberMiddleware.h new file mode 100644 index 00000000000..03f50f757ba --- /dev/null +++ b/LibCarla/source/carla/ros2/dds/fastdds/FastDDSSubscriberMiddleware.h @@ -0,0 +1,151 @@ +// Copyright (c) 2025 Computer Vision Center (CVC) at the Universitat Autonoma de Barcelona (UAB). +// This work is licensed under the terms of the MIT license. +// For a copy, see . + +#pragma once + +#include "carla/ros2/dds/IDDSSubscriberMiddleware.h" +#include "carla/ros2/dds/fastdds/FastDDSTypeMap.h" +#include "carla/Logging.h" + +#include +#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 IDDSSubscriberMiddleware. +/// Parameterized on traits type S that provides: +/// S::msg_type — the message type +/// Native FastDDS types are resolved via FastDDSTypeMap. +template +class FastDDSSubscriberMiddleware + : public IDDSSubscriberMiddleware, + public eprosima::fastdds::dds::DataReaderListener { + public: + using msg_type = typename S::msg_type; + using type_map = FastDDSTypeMap; + using fastdds_type = typename type_map::fastdds_type; + using fastdds_pubsub_type = typename type_map::fastdds_pubsub_type; + + void on_subscription_matched( + efd::DataReader* reader, + const efd::SubscriptionMatchedStatus& info) override { + _alive = (info.total_count > 0); + } + + void on_data_available(efd::DataReader* reader) override { + efd::SampleInfo info; + erc rcode = reader->take_next_sample(&_fastdds_msg, &info); + if (rcode == erc::ReturnCodeValue::RETCODE_OK) { + from_fastdds(_fastdds_msg, *_message_ptr); + *_new_message_ptr = true; + } else { + log_error("FastDDSSubscriberMiddleware::on_data_available (", + _topic_name, ") failed with code:", rcode()); + } + } + + ~FastDDSSubscriberMiddleware() 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( + 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; + } + + efd::DomainParticipantQos pqos = efd::PARTICIPANT_QOS_DEFAULT; + auto factory = efd::DomainParticipantFactory::get_instance(); + _participant = factory->create_participant(0, pqos); + if (_participant == nullptr) { + log_error("FastDDSSubscriberMiddleware: Failed to create DomainParticipant"); + return false; + } + _type.register_type(_participant); + + 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; + } + + 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; + 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; + } + + 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 fastdds_pubsub_type() }; + + fastdds_type _fastdds_msg; + msg_type* _message_ptr { nullptr }; + bool* _new_message_ptr { nullptr }; + + std::string _topic_name; + bool _alive { false }; +}; + +} // namespace ros2 +} // namespace carla diff --git a/LibCarla/source/carla/ros2/dds/fastdds/FastDDSTypeMap.h b/LibCarla/source/carla/ros2/dds/fastdds/FastDDSTypeMap.h new file mode 100644 index 00000000000..7a292f3a2b1 --- /dev/null +++ b/LibCarla/source/carla/ros2/dds/fastdds/FastDDSTypeMap.h @@ -0,0 +1,96 @@ +// 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 . + +// Identity-mapping version: msg_type IS the FastDDS type, so each +// specialization maps a type to itself. + +#pragma once + +// PubSubType headers for type registration +#include "carla/ros2/types/NavSatFixPubSubTypes.h" +#include "carla/ros2/types/ImagePubSubTypes.h" +#include "carla/ros2/types/CameraInfoPubSubTypes.h" +#include "carla/ros2/types/ImuPubSubTypes.h" +#include "carla/ros2/types/PointCloud2PubSubTypes.h" +#include "carla/ros2/types/ClockPubSubTypes.h" +#include "carla/ros2/types/TFMessagePubSubTypes.h" +#include "carla/ros2/types/CarlaCollisionEventPubSubTypes.h" +#include "carla/ros2/types/CarlaEgoVehicleControlPubSubTypes.h" +#include "carla/ros2/types/AckermannDriveStampedPubSubTypes.h" + +namespace carla { +namespace ros2 { + +/// Maps a message type to its FastDDS native type and PubSubType. +/// Primary template is intentionally undefined — only specializations are valid. +template struct FastDDSTypeMap; + +// --- Identity specializations --- +// In the current codebase, msg_type is already the FastDDS-generated type. +// Each specialization maps the type to itself and its corresponding PubSubType. + +template<> struct FastDDSTypeMap { + using fastdds_type = sensor_msgs::msg::NavSatFix; + using fastdds_pubsub_type = sensor_msgs::msg::NavSatFixPubSubType; +}; + +template<> struct FastDDSTypeMap { + using fastdds_type = sensor_msgs::msg::Image; + using fastdds_pubsub_type = sensor_msgs::msg::ImagePubSubType; +}; + +template<> struct FastDDSTypeMap { + using fastdds_type = sensor_msgs::msg::CameraInfo; + using fastdds_pubsub_type = sensor_msgs::msg::CameraInfoPubSubType; +}; + +template<> struct FastDDSTypeMap { + using fastdds_type = sensor_msgs::msg::Imu; + using fastdds_pubsub_type = sensor_msgs::msg::ImuPubSubType; +}; + +template<> struct FastDDSTypeMap { + using fastdds_type = sensor_msgs::msg::PointCloud2; + using fastdds_pubsub_type = sensor_msgs::msg::PointCloud2PubSubType; +}; + +template<> struct FastDDSTypeMap { + using fastdds_type = rosgraph::msg::Clock; + using fastdds_pubsub_type = rosgraph::msg::ClockPubSubType; +}; + +template<> struct FastDDSTypeMap { + using fastdds_type = tf2_msgs::msg::TFMessage; + using fastdds_pubsub_type = tf2_msgs::msg::TFMessagePubSubType; +}; + +template<> struct FastDDSTypeMap { + using fastdds_type = carla_msgs::msg::CarlaCollisionEvent; + using fastdds_pubsub_type = carla_msgs::msg::CarlaCollisionEventPubSubType; +}; + +template<> struct FastDDSTypeMap { + using fastdds_type = carla_msgs::msg::CarlaEgoVehicleControl; + using fastdds_pubsub_type = carla_msgs::msg::CarlaEgoVehicleControlPubSubType; +}; + +template<> struct FastDDSTypeMap { + using fastdds_type = ackermann_msgs::msg::AckermannDriveStamped; + using fastdds_pubsub_type = ackermann_msgs::msg::AckermannDriveStampedPubSubType; +}; + +/// Identity conversion: copy src to dst when both types are the same. +/// In the next phase, we will replace these with real POD-to-FastDDS conversions. +template +inline void to_fastdds(const T& src, T& dst) { + dst = src; +} + +template +inline void from_fastdds(const T& src, T& dst) { + dst = src; +} + +} // namespace ros2 +} // namespace carla diff --git a/LibCarla/source/carla/ros2/publishers/PublisherImpl.h b/LibCarla/source/carla/ros2/publishers/PublisherImpl.h index 377058822fe..9d46419eab7 100644 --- a/LibCarla/source/carla/ros2/publishers/PublisherImpl.h +++ b/LibCarla/source/carla/ros2/publishers/PublisherImpl.h @@ -5,112 +5,40 @@ #pragma once #include +#include -#include -#include -#include - -#include -#include - -#include -#include -#include -#include - -#include - -#include - -#include -#include - +#include "carla/ros2/dds/DDSMiddlewareFactory.h" #include "carla/Logging.h" namespace carla { namespace ros2 { - namespace efd = eprosima::fastdds::dds; - using erc = eprosima::fastrtps::types::ReturnCode_t; - template - class PublisherImpl : public eprosima::fastdds::dds::DataWriterListener { + class PublisherImpl { public: using msg_type = typename T::msg_type; - using msg_pubsub_type = typename T::msg_pubsub_type; - - efd::DomainParticipant* _participant { nullptr }; - efd::Publisher* _publisher { nullptr }; - efd::Topic* _topic { nullptr }; - efd::DataWriter* _datawriter { nullptr }; - efd::TypeSupport _type { new msg_pubsub_type() }; - - void on_publication_matched(efd::DataWriter* /*writer*/, const efd::PublicationMatchedStatus& info) override { - _alive = (info.total_count > 0) ? true : false; - } - - ~PublisherImpl() { - 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("Invalid TypeSupport"); + _middleware = DDSMiddlewareFactory::CreatePublisher(); + if (!_middleware) { + log_error("PublisherImpl: Failed to create middleware publisher"); 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("Failed to create DomainParticipant"); - return false; - } - _type.register_type(_participant); - - efd::PublisherQos pubqos = efd::PUBLISHER_QOS_DEFAULT; - _publisher = _participant->create_publisher(pubqos, nullptr); - if (_publisher == nullptr) { - log_error("Failed to create Publisher"); - return false; - } - - efd::TopicQos tqos = efd::TOPIC_QOS_DEFAULT; - _topic = _participant->create_topic(topic_name, _type->getName(), tqos); - if (_topic == nullptr) { - log_error("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; - efd::DataWriterListener* listener = static_cast(this); - _datawriter = _publisher->create_datawriter(_topic, wqos, listener); - if (_datawriter == nullptr) { - std::cerr << "Failed to create DataWriter" << std::endl; - return false; - } - - _topic_name = topic_name; - return true; + return _middleware->Init(topic_name); } std::string GetTopicName() { - return _topic_name; + if (_middleware) { + return _middleware->GetTopicName(); + } + return ""; } bool IsAlive() { - return _alive; + if (_middleware) { + return _middleware->IsAlive(); + } + return false; } msg_type* GetMessage() { @@ -118,20 +46,15 @@ namespace ros2 { } bool Publish() { - eprosima::fastrtps::rtps::InstanceHandle_t instance_handle; - eprosima::fastrtps::types::ReturnCode_t rcode = _datawriter->write(&_message, instance_handle); - if (rcode == eprosima::fastrtps::types::ReturnCode_t::ReturnCodeValue::RETCODE_OK) { - return true; - } else { - log_error("PublisherImpl::Publish (", this->GetTopicName(), ") failed with code:", rcode()); + if (!_middleware) { + log_error("PublisherImpl::Publish called before Init"); return false; } + return _middleware->Publish(&_message); } private: - std::string _topic_name; - - bool _alive { false }; + std::unique_ptr _middleware; msg_type _message; }; diff --git a/LibCarla/source/carla/ros2/subscribers/SubscriberImpl.h b/LibCarla/source/carla/ros2/subscribers/SubscriberImpl.h index c9e489cc135..5d1f21ea5c5 100644 --- a/LibCarla/source/carla/ros2/subscribers/SubscriberImpl.h +++ b/LibCarla/source/carla/ros2/subscribers/SubscriberImpl.h @@ -5,129 +5,41 @@ #pragma once #include +#include #include "carla/ros2/subscribers/BaseSubscriber.h" - -#include -#include -#include - -#include -#include - -#include -#include -#include -#include -#include -#include - -#include - -#include -#include - +#include "carla/ros2/dds/DDSMiddlewareFactory.h" #include "carla/Logging.h" namespace carla { namespace ros2 { - namespace efd = eprosima::fastdds::dds; - using erc = eprosima::fastrtps::types::ReturnCode_t; - template - class SubscriberImpl : public eprosima::fastdds::dds::DataReaderListener { + class SubscriberImpl { public: using msg_type = typename S::msg_type; - using msg_pubsub_type = typename S::msg_pubsub_type; - - efd::DomainParticipant* _participant { nullptr }; - efd::Subscriber* _subscriber { nullptr }; - efd::Topic* _topic { nullptr }; - efd::DataReader* _datareader { nullptr }; - efd::TypeSupport _type { new msg_pubsub_type() }; - - void on_subscription_matched(efd::DataReader* /*reader*/, const efd::SubscriptionMatchedStatus& info) override { - _alive = (info.total_count > 0) ? true : false; - } - - void on_data_available(efd::DataReader* reader) override { - efd::SampleInfo info; - msg_type message; - - eprosima::fastrtps::types::ReturnCode_t rcode = reader->take_next_sample(&_message, &info); - if (rcode == eprosima::fastrtps::types::ReturnCode_t::ReturnCodeValue::RETCODE_OK) { - // TODO: Process messages directly. - _new_message = true; - } else { - log_error("SubscriberImpl::on_data_available (", this->GetTopicName(), ") failed with code:", rcode()); - } - } - - ~SubscriberImpl() { - 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, S *subscriber) { bool Init(std::string topic_name) { - if (_type == nullptr) { - log_error("Invalid TypeSupport"); + _middleware = DDSMiddlewareFactory::CreateSubscriber(); + if (!_middleware) { + log_error("SubscriberImpl: Failed to create middleware subscriber"); 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("Failed to create DomainParticipant"); - return false; - } - _type.register_type(_participant); - - efd::SubscriberQos subqos = efd::SUBSCRIBER_QOS_DEFAULT; - _subscriber = _participant->create_subscriber(subqos, nullptr); - if (_subscriber == nullptr) { - log_error("Failed to create Subscriber"); - return false; - } - - efd::TopicQos tqos = efd::TOPIC_QOS_DEFAULT; - _topic = _participant->create_topic(topic_name, _type->getName(), tqos); - if (_topic == nullptr) { - log_error("Failed to create Topic"); - return false; - } - - 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("Failed to create DataReader"); - return false; - } - - _topic_name = topic_name; - - // _subscriber = subscriber; - return true; + return _middleware->Init(topic_name, &_message, &_new_message); } std::string GetTopicName() { - return _topic_name; + if (_middleware) { + return _middleware->GetTopicName(); + } + return ""; } bool IsAlive() { - return _alive; + if (_middleware) { + return _middleware->IsAlive(); + } + return false; } msg_type GetMessage() { @@ -138,11 +50,9 @@ namespace ros2 { bool HasNewMessage() { return _new_message; } private: - std::string _topic_name; - - bool _alive { false }; - bool _new_message { false }; + std::unique_ptr _middleware; msg_type _message; + bool _new_message { false }; }; } // namespace ros2 From e665d2afc3c3cec94b45de3c8383d7b3192e74dd Mon Sep 17 00:00:00 2001 From: Jesus Armando Anaya Date: Fri, 27 Mar 2026 18:50:41 -0700 Subject: [PATCH 2/7] feat(LibCarla/ros2): add POD message types, FastDDS conversions, and real TypeMap specializations Introduce backend-neutral POD message structs and to_fastdds/from_fastdds conversion functions, completing the type abstraction layer started in PR #1 without touching any concrete publisher or subscriber. New files (LibCarla/source/carla/ros2/types/): - 31 plain C++ structs in types/msg/ - one per ROS2 message type, no DDS dependency, standard library headers only, all members value-initialized - FastDDSConversions.h: inline to_fastdds/from_fastdds overload pairs for all 31 type pairs, ordered bottom-up so composites call primitives; move overloads for msg::Image and msg::PointCloud2 to avoid copying large data vectors New test file (LibCarla/source/test/server/test_dds_middleware.cpp): - 45 unit tests across 10 groups covering the middleware enum, factory, interfaces, PublisherImpl, and SubscriberImpl using hand-written mocks; no DDS daemon required (CARLA_ROS2_DDS_TESTING suppresses real includes) Modified files: - FastDDSTypeMap.h: 28 real specializations mapping msg::X to FastDDS types, coexisting with the existing identity specializations (different key types, no ODR conflict) - DDSMiddleware.h: add ToROS2DDSTypeName() for use in PR #3 - DDSMiddlewareFactory.h: add CARLA_ROS2_DDS_TESTING guard to FastDDS include/create blocks so test binary compiles without real DDS headers - PublisherImpl.h / SubscriberImpl.h: LIBCARLA_WITH_GTEST test seams for middleware injection and message simulation - cmake/fast_dds/CMakeLists.txt: added glob for types/msg/*.h install No behavior change - identity TypeMap specializations remain active; all concrete publishers and subscribers compile and work unchanged. --- LibCarla/cmake/fast_dds/CMakeLists.txt | 1 + .../source/carla/ros2/dds/DDSMiddleware.h | 12 + .../carla/ros2/dds/DDSMiddlewareFactory.h | 6 +- .../carla/ros2/dds/fastdds/FastDDSTypeMap.h | 203 +++++- .../carla/ros2/publishers/PublisherImpl.h | 18 +- .../carla/ros2/subscribers/SubscriberImpl.h | 23 +- .../carla/ros2/types/FastDDSConversions.h | 623 ++++++++++++++++++ .../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 + .../test/server/test_dds_middleware.cpp | 490 ++++++++++++++ 39 files changed, 2049 insertions(+), 19 deletions(-) create mode 100644 LibCarla/source/carla/ros2/types/FastDDSConversions.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 create mode 100644 LibCarla/source/test/server/test_dds_middleware.cpp diff --git a/LibCarla/cmake/fast_dds/CMakeLists.txt b/LibCarla/cmake/fast_dds/CMakeLists.txt index 4ff74c21d57..f51bbc430f7 100644 --- a/LibCarla/cmake/fast_dds/CMakeLists.txt +++ b/LibCarla/cmake/fast_dds/CMakeLists.txt @@ -9,6 +9,7 @@ file(GLOB libcarla_carla_fastdds_headers "${libcarla_source_path}/carla/ros2/subscribers/*.h" "${libcarla_source_path}/carla/ros2/listeners/*.h" "${libcarla_source_path}/carla/ros2/types/*.h" + "${libcarla_source_path}/carla/ros2/types/msg/*.h" "${libcarla_source_path}/carla/ros2/dds/*.h" "${libcarla_source_path}/carla/ros2/dds/fastdds/*.h" ) diff --git a/LibCarla/source/carla/ros2/dds/DDSMiddleware.h b/LibCarla/source/carla/ros2/dds/DDSMiddleware.h index 35a4d89be47..20f8804c628 100644 --- a/LibCarla/source/carla/ros2/dds/DDSMiddleware.h +++ b/LibCarla/source/carla/ros2/dds/DDSMiddleware.h @@ -52,5 +52,17 @@ inline std::string GetAvailableMiddlewareString() { return result; } +/// Mangle a DDS type name into the ROS2-compatible format. +/// "sensor_msgs::msg::Image" becomes "sensor_msgs::msg::dds_::Image_". +/// A bare name like "Image" becomes "dds_::Image_". +inline std::string ToROS2DDSTypeName(const std::string& dds_type_name) { + auto pos = dds_type_name.rfind("::"); + if (pos == std::string::npos) { + return "dds_::" + dds_type_name + "_"; + } + return dds_type_name.substr(0, pos) + + "::dds_::" + dds_type_name.substr(pos + 2) + "_"; +} + } // namespace ros2 } // namespace carla diff --git a/LibCarla/source/carla/ros2/dds/DDSMiddlewareFactory.h b/LibCarla/source/carla/ros2/dds/DDSMiddlewareFactory.h index 5b7b2adcd9a..51cc8f81143 100644 --- a/LibCarla/source/carla/ros2/dds/DDSMiddlewareFactory.h +++ b/LibCarla/source/carla/ros2/dds/DDSMiddlewareFactory.h @@ -12,7 +12,7 @@ #include "carla/ros2/dds/IDDSSubscriberMiddleware.h" #include "carla/Logging.h" -#if defined(CARLA_ROS2_DDS_FASTDDS) +#if defined(CARLA_ROS2_DDS_FASTDDS) && !defined(CARLA_ROS2_DDS_TESTING) # include "carla/ros2/dds/fastdds/FastDDSPublisherMiddleware.h" # include "carla/ros2/dds/fastdds/FastDDSSubscriberMiddleware.h" #endif @@ -77,7 +77,7 @@ class DDSMiddlewareFactory { static std::unique_ptr CreatePublisher() { switch (GetActiveMiddleware()) { case DDSMiddleware::FastDDS: -#if defined(CARLA_ROS2_DDS_FASTDDS) +#if defined(CARLA_ROS2_DDS_FASTDDS) && !defined(CARLA_ROS2_DDS_TESTING) return std::unique_ptr( new FastDDSPublisherMiddleware()); #else @@ -95,7 +95,7 @@ class DDSMiddlewareFactory { static std::unique_ptr CreateSubscriber() { switch (GetActiveMiddleware()) { case DDSMiddleware::FastDDS: -#if defined(CARLA_ROS2_DDS_FASTDDS) +#if defined(CARLA_ROS2_DDS_FASTDDS) && !defined(CARLA_ROS2_DDS_TESTING) return std::unique_ptr( new FastDDSSubscriberMiddleware()); #else diff --git a/LibCarla/source/carla/ros2/dds/fastdds/FastDDSTypeMap.h b/LibCarla/source/carla/ros2/dds/fastdds/FastDDSTypeMap.h index 7a292f3a2b1..1a9ad1935a1 100644 --- a/LibCarla/source/carla/ros2/dds/fastdds/FastDDSTypeMap.h +++ b/LibCarla/source/carla/ros2/dds/fastdds/FastDDSTypeMap.h @@ -2,22 +2,42 @@ // This work is licensed under the terms of the MIT license. // For a copy, see . -// Identity-mapping version: msg_type IS the FastDDS type, so each -// specialization maps a type to itself. - #pragma once +#include "carla/ros2/types/FastDDSConversions.h" + // PubSubType headers for type registration +#include "carla/ros2/types/TimePubSubTypes.h" +#include "carla/ros2/types/HeaderPubSubTypes.h" +#include "carla/ros2/types/Vector3PubSubTypes.h" +#include "carla/ros2/types/QuaternionPubSubTypes.h" +#include "carla/ros2/types/PointPubSubTypes.h" +#include "carla/ros2/types/Point32PubSubTypes.h" +#include "carla/ros2/types/PosePubSubTypes.h" +#include "carla/ros2/types/PoseWithCovariancePubSubTypes.h" +#include "carla/ros2/types/TwistPubSubTypes.h" +#include "carla/ros2/types/TwistWithCovariancePubSubTypes.h" +#include "carla/ros2/types/TransformPubSubTypes.h" +#include "carla/ros2/types/TransformStampedPubSubTypes.h" +#include "carla/ros2/types/OdometryPubSubTypes.h" +#include "carla/ros2/types/RegionOfInterestPubSubTypes.h" +#include "carla/ros2/types/PointFieldPubSubTypes.h" +#include "carla/ros2/types/NavSatStatusPubSubTypes.h" #include "carla/ros2/types/NavSatFixPubSubTypes.h" +#include "carla/ros2/types/ClockPubSubTypes.h" +#include "carla/ros2/types/Float32PubSubTypes.h" +#include "carla/ros2/types/StringPubSubTypes.h" +#include "carla/ros2/types/ImuPubSubTypes.h" #include "carla/ros2/types/ImagePubSubTypes.h" #include "carla/ros2/types/CameraInfoPubSubTypes.h" -#include "carla/ros2/types/ImuPubSubTypes.h" #include "carla/ros2/types/PointCloud2PubSubTypes.h" -#include "carla/ros2/types/ClockPubSubTypes.h" #include "carla/ros2/types/TFMessagePubSubTypes.h" +#include "carla/ros2/types/TF2ErrorPubSubTypes.h" +#include "carla/ros2/types/AckermannDrivePubSubTypes.h" +#include "carla/ros2/types/AckermannDriveStampedPubSubTypes.h" #include "carla/ros2/types/CarlaCollisionEventPubSubTypes.h" #include "carla/ros2/types/CarlaEgoVehicleControlPubSubTypes.h" -#include "carla/ros2/types/AckermannDriveStampedPubSubTypes.h" +#include "carla/ros2/types/CarlaLineInvasionPubSubTypes.h" namespace carla { namespace ros2 { @@ -26,9 +46,11 @@ namespace ros2 { /// Primary template is intentionally undefined — only specializations are valid. template struct FastDDSTypeMap; -// --- Identity specializations --- -// In the current codebase, msg_type is already the FastDDS-generated type. -// Each specialization maps the type to itself and its corresponding PubSubType. +// ============================================================ +// Identity specializations (msg_type IS the FastDDS type) +// Used by current publishers/subscribers until they are migrated +// to POD message types in PR #2b. +// ============================================================ template<> struct FastDDSTypeMap { using fastdds_type = sensor_msgs::msg::NavSatFix; @@ -81,7 +103,7 @@ template<> struct FastDDSTypeMap { }; /// Identity conversion: copy src to dst when both types are the same. -/// In the next phase, we will replace these with real POD-to-FastDDS conversions. +/// Used by publishers/subscribers that have not yet migrated to POD types. template inline void to_fastdds(const T& src, T& dst) { dst = src; @@ -92,5 +114,166 @@ inline void from_fastdds(const T& src, T& dst) { dst = src; } +// ============================================================ +// Real specializations (POD msg types -> FastDDS types) +// These map backend-neutral POD structs to FastDDS-generated types. +// Conversion functions are in types/FastDDSConversions.h. +// ============================================================ + +template<> struct FastDDSTypeMap { + using fastdds_type = builtin_interfaces::msg::Time; + using fastdds_pubsub_type = builtin_interfaces::msg::TimePubSubType; +}; + +template<> struct FastDDSTypeMap { + using fastdds_type = std_msgs::msg::Header; + using fastdds_pubsub_type = std_msgs::msg::HeaderPubSubType; +}; + +template<> struct FastDDSTypeMap { + using fastdds_type = geometry_msgs::msg::Vector3; + using fastdds_pubsub_type = geometry_msgs::msg::Vector3PubSubType; +}; + +template<> struct FastDDSTypeMap { + using fastdds_type = geometry_msgs::msg::Quaternion; + using fastdds_pubsub_type = geometry_msgs::msg::QuaternionPubSubType; +}; + +template<> struct FastDDSTypeMap { + using fastdds_type = geometry_msgs::msg::Point; + using fastdds_pubsub_type = geometry_msgs::msg::PointPubSubType; +}; + +template<> struct FastDDSTypeMap { + using fastdds_type = geometry_msgs::msg::Point32; + using fastdds_pubsub_type = geometry_msgs::msg::Point32PubSubType; +}; + +template<> struct FastDDSTypeMap { + using fastdds_type = geometry_msgs::msg::Pose; + using fastdds_pubsub_type = geometry_msgs::msg::PosePubSubType; +}; + +template<> struct FastDDSTypeMap { + using fastdds_type = geometry_msgs::msg::PoseWithCovariance; + using fastdds_pubsub_type = geometry_msgs::msg::PoseWithCovariancePubSubType; +}; + +template<> struct FastDDSTypeMap { + using fastdds_type = geometry_msgs::msg::Twist; + using fastdds_pubsub_type = geometry_msgs::msg::TwistPubSubType; +}; + +template<> struct FastDDSTypeMap { + using fastdds_type = geometry_msgs::msg::TwistWithCovariance; + using fastdds_pubsub_type = geometry_msgs::msg::TwistWithCovariancePubSubType; +}; + +template<> struct FastDDSTypeMap { + using fastdds_type = geometry_msgs::msg::Transform; + using fastdds_pubsub_type = geometry_msgs::msg::TransformPubSubType; +}; + +template<> struct FastDDSTypeMap { + using fastdds_type = geometry_msgs::msg::TransformStamped; + using fastdds_pubsub_type = geometry_msgs::msg::TransformStampedPubSubType; +}; + +template<> struct FastDDSTypeMap { + using fastdds_type = nav_msgs::msg::Odometry; + using fastdds_pubsub_type = nav_msgs::msg::OdometryPubSubType; +}; + +template<> struct FastDDSTypeMap { + using fastdds_type = sensor_msgs::msg::RegionOfInterest; + using fastdds_pubsub_type = sensor_msgs::msg::RegionOfInterestPubSubType; +}; + +template<> struct FastDDSTypeMap { + using fastdds_type = sensor_msgs::msg::PointField; + using fastdds_pubsub_type = sensor_msgs::msg::PointFieldPubSubType; +}; + +template<> struct FastDDSTypeMap { + using fastdds_type = sensor_msgs::msg::NavSatStatus; + using fastdds_pubsub_type = sensor_msgs::msg::NavSatStatusPubSubType; +}; + +template<> struct FastDDSTypeMap { + using fastdds_type = sensor_msgs::msg::NavSatFix; + using fastdds_pubsub_type = sensor_msgs::msg::NavSatFixPubSubType; +}; + +template<> struct FastDDSTypeMap { + using fastdds_type = rosgraph::msg::Clock; + using fastdds_pubsub_type = rosgraph::msg::ClockPubSubType; +}; + +template<> struct FastDDSTypeMap { + using fastdds_type = std_msgs::msg::Float32; + using fastdds_pubsub_type = std_msgs::msg::Float32PubSubType; +}; + +template<> struct FastDDSTypeMap { + using fastdds_type = std_msgs::msg::String; + using fastdds_pubsub_type = std_msgs::msg::StringPubSubType; +}; + +template<> struct FastDDSTypeMap { + using fastdds_type = sensor_msgs::msg::Imu; + using fastdds_pubsub_type = sensor_msgs::msg::ImuPubSubType; +}; + +template<> struct FastDDSTypeMap { + using fastdds_type = sensor_msgs::msg::Image; + using fastdds_pubsub_type = sensor_msgs::msg::ImagePubSubType; +}; + +template<> struct FastDDSTypeMap { + using fastdds_type = sensor_msgs::msg::CameraInfo; + using fastdds_pubsub_type = sensor_msgs::msg::CameraInfoPubSubType; +}; + +template<> struct FastDDSTypeMap { + using fastdds_type = sensor_msgs::msg::PointCloud2; + using fastdds_pubsub_type = sensor_msgs::msg::PointCloud2PubSubType; +}; + +template<> struct FastDDSTypeMap { + using fastdds_type = tf2_msgs::msg::TFMessage; + using fastdds_pubsub_type = tf2_msgs::msg::TFMessagePubSubType; +}; + +template<> struct FastDDSTypeMap { + using fastdds_type = tf2_msgs::msg::TF2Error; + using fastdds_pubsub_type = tf2_msgs::msg::TF2ErrorPubSubType; +}; + +template<> struct FastDDSTypeMap { + using fastdds_type = ackermann_msgs::msg::AckermannDrive; + using fastdds_pubsub_type = ackermann_msgs::msg::AckermannDrivePubSubType; +}; + +template<> struct FastDDSTypeMap { + using fastdds_type = ackermann_msgs::msg::AckermannDriveStamped; + using fastdds_pubsub_type = ackermann_msgs::msg::AckermannDriveStampedPubSubType; +}; + +template<> struct FastDDSTypeMap { + using fastdds_type = carla_msgs::msg::CarlaCollisionEvent; + using fastdds_pubsub_type = carla_msgs::msg::CarlaCollisionEventPubSubType; +}; + +template<> struct FastDDSTypeMap { + using fastdds_type = carla_msgs::msg::CarlaEgoVehicleControl; + using fastdds_pubsub_type = carla_msgs::msg::CarlaEgoVehicleControlPubSubType; +}; + +template<> struct FastDDSTypeMap { + using fastdds_type = carla_msgs::msg::LaneInvasionEvent; + using fastdds_pubsub_type = carla_msgs::msg::LaneInvasionEventPubSubType; +}; + } // namespace ros2 } // namespace carla diff --git a/LibCarla/source/carla/ros2/publishers/PublisherImpl.h b/LibCarla/source/carla/ros2/publishers/PublisherImpl.h index 9d46419eab7..63495013725 100644 --- a/LibCarla/source/carla/ros2/publishers/PublisherImpl.h +++ b/LibCarla/source/carla/ros2/publishers/PublisherImpl.h @@ -19,11 +19,17 @@ namespace ros2 { using msg_type = typename T::msg_type; bool Init(std::string topic_name) { - _middleware = DDSMiddlewareFactory::CreatePublisher(); +#ifdef LIBCARLA_WITH_GTEST if (!_middleware) { - log_error("PublisherImpl: Failed to create middleware publisher"); - return false; +#endif + _middleware = DDSMiddlewareFactory::CreatePublisher(); + if (!_middleware) { + log_error("PublisherImpl: Failed to create middleware publisher"); + return false; + } +#ifdef LIBCARLA_WITH_GTEST } +#endif return _middleware->Init(topic_name); } @@ -53,6 +59,12 @@ namespace ros2 { return _middleware->Publish(&_message); } +#ifdef LIBCARLA_WITH_GTEST + void SetMiddlewareForTesting(std::unique_ptr middleware) { + _middleware = std::move(middleware); + } +#endif + private: std::unique_ptr _middleware; msg_type _message; diff --git a/LibCarla/source/carla/ros2/subscribers/SubscriberImpl.h b/LibCarla/source/carla/ros2/subscribers/SubscriberImpl.h index 5d1f21ea5c5..8938496d64e 100644 --- a/LibCarla/source/carla/ros2/subscribers/SubscriberImpl.h +++ b/LibCarla/source/carla/ros2/subscribers/SubscriberImpl.h @@ -20,11 +20,17 @@ namespace ros2 { using msg_type = typename S::msg_type; bool Init(std::string topic_name) { - _middleware = DDSMiddlewareFactory::CreateSubscriber(); +#ifdef LIBCARLA_WITH_GTEST if (!_middleware) { - log_error("SubscriberImpl: Failed to create middleware subscriber"); - return false; +#endif + _middleware = DDSMiddlewareFactory::CreateSubscriber(); + if (!_middleware) { + log_error("SubscriberImpl: Failed to create middleware subscriber"); + return false; + } +#ifdef LIBCARLA_WITH_GTEST } +#endif return _middleware->Init(topic_name, &_message, &_new_message); } @@ -49,6 +55,17 @@ namespace ros2 { bool HasNewMessage() { return _new_message; } +#ifdef LIBCARLA_WITH_GTEST + void SetMiddlewareForTesting(std::unique_ptr middleware) { + _middleware = std::move(middleware); + } + + void SimulateMessageReceiptForTesting(const msg_type& msg) { + _message = msg; + _new_message = true; + } +#endif + private: std::unique_ptr _middleware; msg_type _message; diff --git a/LibCarla/source/carla/ros2/types/FastDDSConversions.h b/LibCarla/source/carla/ros2/types/FastDDSConversions.h new file mode 100644 index 00000000000..9fdb7739ed7 --- /dev/null +++ b/LibCarla/source/carla/ros2/types/FastDDSConversions.h @@ -0,0 +1,623 @@ +// 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 + +// POD message types +#include "carla/ros2/types/msg/Time.h" +#include "carla/ros2/types/msg/Header.h" +#include "carla/ros2/types/msg/Vector3.h" +#include "carla/ros2/types/msg/Quaternion.h" +#include "carla/ros2/types/msg/Point.h" +#include "carla/ros2/types/msg/Point32.h" +#include "carla/ros2/types/msg/Pose.h" +#include "carla/ros2/types/msg/PoseWithCovariance.h" +#include "carla/ros2/types/msg/Twist.h" +#include "carla/ros2/types/msg/TwistWithCovariance.h" +#include "carla/ros2/types/msg/Transform.h" +#include "carla/ros2/types/msg/TransformStamped.h" +#include "carla/ros2/types/msg/Odometry.h" +#include "carla/ros2/types/msg/RegionOfInterest.h" +#include "carla/ros2/types/msg/PointField.h" +#include "carla/ros2/types/msg/NavSatStatus.h" +#include "carla/ros2/types/msg/NavSatFix.h" +#include "carla/ros2/types/msg/Clock.h" +#include "carla/ros2/types/msg/Float32.h" +#include "carla/ros2/types/msg/String.h" +#include "carla/ros2/types/msg/Imu.h" +#include "carla/ros2/types/msg/Image.h" +#include "carla/ros2/types/msg/CameraInfo.h" +#include "carla/ros2/types/msg/PointCloud2.h" +#include "carla/ros2/types/msg/TFMessage.h" +#include "carla/ros2/types/msg/TF2Error.h" +#include "carla/ros2/types/msg/AckermannDrive.h" +#include "carla/ros2/types/msg/AckermannDriveStamped.h" +#include "carla/ros2/types/msg/CarlaCollisionEvent.h" +#include "carla/ros2/types/msg/CarlaEgoVehicleControl.h" +#include "carla/ros2/types/msg/CarlaLineInvasion.h" + +// FastDDS-generated types (current flat layout) +#include "carla/ros2/types/Time.h" +#include "carla/ros2/types/Header.h" +#include "carla/ros2/types/Vector3.h" +#include "carla/ros2/types/Quaternion.h" +#include "carla/ros2/types/Point.h" +#include "carla/ros2/types/Point32.h" +#include "carla/ros2/types/Pose.h" +#include "carla/ros2/types/PoseWithCovariance.h" +#include "carla/ros2/types/Twist.h" +#include "carla/ros2/types/TwistWithCovariance.h" +#include "carla/ros2/types/Transform.h" +#include "carla/ros2/types/TransformStamped.h" +#include "carla/ros2/types/Odometry.h" +#include "carla/ros2/types/RegionOfInterest.h" +#include "carla/ros2/types/PointField.h" +#include "carla/ros2/types/NavSatStatus.h" +#include "carla/ros2/types/NavSatFix.h" +#include "carla/ros2/types/Clock.h" +#include "carla/ros2/types/Float32.h" +#include "carla/ros2/types/String.h" +#include "carla/ros2/types/Imu.h" +#include "carla/ros2/types/Image.h" +#include "carla/ros2/types/CameraInfo.h" +#include "carla/ros2/types/PointCloud2.h" +#include "carla/ros2/types/TFMessage.h" +#include "carla/ros2/types/TF2Error.h" +#include "carla/ros2/types/AckermannDrive.h" +#include "carla/ros2/types/AckermannDriveStamped.h" +#include "carla/ros2/types/CarlaCollisionEvent.h" +#include "carla/ros2/types/CarlaEgoVehicleControl.h" +#include "carla/ros2/types/CarlaLineInvasion.h" + +#include + +namespace carla { +namespace ros2 { + +// ============================================================ +// Conversion functions: POD msg types <-> FastDDS-generated types +// Order matters — composites call primitives. +// ============================================================ + +// --- Primitives --- + +inline void to_fastdds(const msg::Time& src, builtin_interfaces::msg::Time& dst) { + dst.sec(src.sec); + dst.nanosec(src.nanosec); +} + +inline void from_fastdds(const builtin_interfaces::msg::Time& src, msg::Time& dst) { + dst.sec = src.sec(); + dst.nanosec = src.nanosec(); +} + +inline void to_fastdds(const msg::Header& src, std_msgs::msg::Header& dst) { + to_fastdds(src.stamp, dst.stamp()); + dst.frame_id(src.frame_id); +} + +inline void from_fastdds(const std_msgs::msg::Header& src, msg::Header& dst) { + from_fastdds(src.stamp(), dst.stamp); + dst.frame_id = src.frame_id(); +} + +inline void to_fastdds(const msg::Vector3& src, geometry_msgs::msg::Vector3& dst) { + dst.x(src.x); + dst.y(src.y); + dst.z(src.z); +} + +inline void from_fastdds(const geometry_msgs::msg::Vector3& src, msg::Vector3& dst) { + dst.x = src.x(); + dst.y = src.y(); + dst.z = src.z(); +} + +inline void to_fastdds( + const msg::Quaternion& src, + geometry_msgs::msg::Quaternion& dst) { + dst.x(src.x); + dst.y(src.y); + dst.z(src.z); + dst.w(src.w); +} + +inline void from_fastdds( + const geometry_msgs::msg::Quaternion& src, + msg::Quaternion& dst) { + dst.x = src.x(); + dst.y = src.y(); + dst.z = src.z(); + dst.w = src.w(); +} + +inline void to_fastdds(const msg::Point& src, geometry_msgs::msg::Point& dst) { + dst.x(src.x); + dst.y(src.y); + dst.z(src.z); +} + +inline void from_fastdds(const geometry_msgs::msg::Point& src, msg::Point& dst) { + dst.x = src.x(); + dst.y = src.y(); + dst.z = src.z(); +} + +inline void to_fastdds(const msg::Point32& src, geometry_msgs::msg::Point32& dst) { + dst.x(src.x); + dst.y(src.y); + dst.z(src.z); +} + +inline void from_fastdds(const geometry_msgs::msg::Point32& src, msg::Point32& dst) { + dst.x = src.x(); + dst.y = src.y(); + dst.z = src.z(); +} + +// --- Geometry composites --- + +inline void to_fastdds(const msg::Pose& src, geometry_msgs::msg::Pose& dst) { + to_fastdds(src.position, dst.position()); + to_fastdds(src.orientation, dst.orientation()); +} + +inline void from_fastdds(const geometry_msgs::msg::Pose& src, msg::Pose& dst) { + from_fastdds(src.position(), dst.position); + from_fastdds(src.orientation(), dst.orientation); +} + +inline void to_fastdds( + const msg::PoseWithCovariance& src, + geometry_msgs::msg::PoseWithCovariance& dst) { + to_fastdds(src.pose, dst.pose()); + dst.covariance(src.covariance); +} + +inline void from_fastdds( + const geometry_msgs::msg::PoseWithCovariance& src, + msg::PoseWithCovariance& dst) { + from_fastdds(src.pose(), dst.pose); + dst.covariance = src.covariance(); +} + +inline void to_fastdds(const msg::Twist& src, geometry_msgs::msg::Twist& dst) { + to_fastdds(src.linear, dst.linear()); + to_fastdds(src.angular, dst.angular()); +} + +inline void from_fastdds(const geometry_msgs::msg::Twist& src, msg::Twist& dst) { + from_fastdds(src.linear(), dst.linear); + from_fastdds(src.angular(), dst.angular); +} + +inline void to_fastdds( + const msg::TwistWithCovariance& src, + geometry_msgs::msg::TwistWithCovariance& dst) { + to_fastdds(src.twist, dst.twist()); + dst.covariance(src.covariance); +} + +inline void from_fastdds( + const geometry_msgs::msg::TwistWithCovariance& src, + msg::TwistWithCovariance& dst) { + from_fastdds(src.twist(), dst.twist); + dst.covariance = src.covariance(); +} + +inline void to_fastdds( + const msg::Transform& src, + geometry_msgs::msg::Transform& dst) { + to_fastdds(src.translation, dst.translation()); + to_fastdds(src.rotation, dst.rotation()); +} + +inline void from_fastdds( + const geometry_msgs::msg::Transform& src, + msg::Transform& dst) { + from_fastdds(src.translation(), dst.translation); + from_fastdds(src.rotation(), dst.rotation); +} + +inline void to_fastdds( + const msg::TransformStamped& src, + geometry_msgs::msg::TransformStamped& dst) { + to_fastdds(src.header, dst.header()); + dst.child_frame_id(src.child_frame_id); + to_fastdds(src.transform, dst.transform()); +} + +inline void from_fastdds( + const geometry_msgs::msg::TransformStamped& src, + msg::TransformStamped& dst) { + from_fastdds(src.header(), dst.header); + dst.child_frame_id = src.child_frame_id(); + from_fastdds(src.transform(), dst.transform); +} + +// --- Sensor primitives --- + +inline void to_fastdds( + const msg::RegionOfInterest& src, + sensor_msgs::msg::RegionOfInterest& dst) { + dst.x_offset(src.x_offset); + dst.y_offset(src.y_offset); + dst.height(src.height); + dst.width(src.width); + dst.do_rectify(src.do_rectify); +} + +inline void from_fastdds( + const sensor_msgs::msg::RegionOfInterest& src, + msg::RegionOfInterest& dst) { + dst.x_offset = src.x_offset(); + dst.y_offset = src.y_offset(); + dst.height = src.height(); + dst.width = src.width(); + dst.do_rectify = src.do_rectify(); +} + +inline void to_fastdds( + const msg::PointField& src, + sensor_msgs::msg::PointField& dst) { + dst.name(src.name); + dst.offset(src.offset); + dst.datatype(src.datatype); + dst.count(src.count); +} + +inline void from_fastdds( + const sensor_msgs::msg::PointField& src, + msg::PointField& dst) { + dst.name = src.name(); + dst.offset = src.offset(); + dst.datatype = src.datatype(); + dst.count = src.count(); +} + +inline void to_fastdds( + const msg::NavSatStatus& src, + sensor_msgs::msg::NavSatStatus& dst) { + dst.status(src.status); + dst.service(src.service); +} + +inline void from_fastdds( + const sensor_msgs::msg::NavSatStatus& src, + msg::NavSatStatus& dst) { + dst.status = src.status(); + dst.service = src.service(); +} + +// --- Simple messages --- + +inline void to_fastdds(const msg::Clock& src, rosgraph::msg::Clock& dst) { + to_fastdds(src.clock, dst.clock()); +} + +inline void from_fastdds(const rosgraph::msg::Clock& src, msg::Clock& dst) { + from_fastdds(src.clock(), dst.clock); +} + +inline void to_fastdds(const msg::Float32& src, std_msgs::msg::Float32& dst) { + dst.data(src.data); +} + +inline void from_fastdds(const std_msgs::msg::Float32& src, msg::Float32& dst) { + dst.data = src.data(); +} + +inline void to_fastdds(const msg::String& src, std_msgs::msg::String& dst) { + dst.data(src.data); +} + +inline void from_fastdds(const std_msgs::msg::String& src, msg::String& dst) { + dst.data = src.data(); +} + +// --- Complex sensor messages --- + +inline void to_fastdds(const msg::Imu& src, sensor_msgs::msg::Imu& dst) { + to_fastdds(src.header, dst.header()); + to_fastdds(src.orientation, dst.orientation()); + dst.orientation_covariance(src.orientation_covariance); + to_fastdds(src.angular_velocity, dst.angular_velocity()); + dst.angular_velocity_covariance(src.angular_velocity_covariance); + to_fastdds(src.linear_acceleration, dst.linear_acceleration()); + dst.linear_acceleration_covariance(src.linear_acceleration_covariance); +} + +inline void from_fastdds(const sensor_msgs::msg::Imu& src, msg::Imu& dst) { + from_fastdds(src.header(), dst.header); + from_fastdds(src.orientation(), dst.orientation); + dst.orientation_covariance = src.orientation_covariance(); + from_fastdds(src.angular_velocity(), dst.angular_velocity); + dst.angular_velocity_covariance = src.angular_velocity_covariance(); + from_fastdds(src.linear_acceleration(), dst.linear_acceleration); + dst.linear_acceleration_covariance = src.linear_acceleration_covariance(); +} + +inline void to_fastdds( + const msg::NavSatFix& src, + sensor_msgs::msg::NavSatFix& dst) { + to_fastdds(src.header, dst.header()); + to_fastdds(src.status, dst.status()); + dst.latitude(src.latitude); + dst.longitude(src.longitude); + dst.altitude(src.altitude); + dst.position_covariance(src.position_covariance); + dst.position_covariance_type(src.position_covariance_type); +} + +inline void from_fastdds( + const sensor_msgs::msg::NavSatFix& src, + msg::NavSatFix& dst) { + from_fastdds(src.header(), dst.header); + from_fastdds(src.status(), dst.status); + dst.latitude = src.latitude(); + dst.longitude = src.longitude(); + dst.altitude = src.altitude(); + dst.position_covariance = src.position_covariance(); + dst.position_covariance_type = src.position_covariance_type(); +} + +// Image: const-ref overload (copies data vector) +inline void to_fastdds(const msg::Image& src, sensor_msgs::msg::Image& dst) { + to_fastdds(src.header, dst.header()); + dst.height(src.height); + dst.width(src.width); + dst.encoding(src.encoding); + dst.is_bigendian(src.is_bigendian); + dst.step(src.step); + dst.data(src.data); +} + +// Image: non-const overload (moves data vector) +inline void to_fastdds(msg::Image& src, sensor_msgs::msg::Image& dst) { + to_fastdds(src.header, dst.header()); + dst.height(src.height); + dst.width(src.width); + dst.encoding(std::move(src.encoding)); + dst.is_bigendian(src.is_bigendian); + dst.step(src.step); + dst.data(std::move(src.data)); +} + +inline void from_fastdds(const sensor_msgs::msg::Image& src, msg::Image& dst) { + from_fastdds(src.header(), dst.header); + dst.height = src.height(); + dst.width = src.width(); + dst.encoding = src.encoding(); + dst.is_bigendian = src.is_bigendian(); + dst.step = src.step(); + dst.data = src.data(); +} + +inline void to_fastdds( + const msg::CameraInfo& src, + sensor_msgs::msg::CameraInfo& dst) { + to_fastdds(src.header, dst.header()); + dst.height(src.height); + dst.width(src.width); + dst.distortion_model(src.distortion_model); + dst.D(src.d); + dst.k(src.k); + dst.r(src.r); + dst.p(src.p); + dst.binning_x(src.binning_x); + dst.binning_y(src.binning_y); + to_fastdds(src.roi, dst.roi()); +} + +inline void from_fastdds( + const sensor_msgs::msg::CameraInfo& src, + msg::CameraInfo& dst) { + from_fastdds(src.header(), dst.header); + dst.height = src.height(); + dst.width = src.width(); + dst.distortion_model = src.distortion_model(); + dst.d = src.D(); + dst.k = src.k(); + dst.r = src.r(); + dst.p = src.p(); + dst.binning_x = src.binning_x(); + dst.binning_y = src.binning_y(); + from_fastdds(src.roi(), dst.roi); +} + +// PointCloud2: const-ref overload (copies data vector) +inline void to_fastdds( + const msg::PointCloud2& src, + sensor_msgs::msg::PointCloud2& dst) { + to_fastdds(src.header, dst.header()); + dst.height(src.height); + dst.width(src.width); + std::vector fields(src.fields.size()); + for (size_t i = 0; i < src.fields.size(); ++i) { + to_fastdds(src.fields[i], fields[i]); + } + dst.fields(std::move(fields)); + dst.is_bigendian(src.is_bigendian); + dst.point_step(src.point_step); + dst.row_step(src.row_step); + dst.data(src.data); + dst.is_dense(src.is_dense); +} + +// PointCloud2: non-const overload (moves data vector) +inline void to_fastdds( + msg::PointCloud2& src, + sensor_msgs::msg::PointCloud2& dst) { + to_fastdds(src.header, dst.header()); + dst.height(src.height); + dst.width(src.width); + std::vector fields(src.fields.size()); + for (size_t i = 0; i < src.fields.size(); ++i) { + to_fastdds(src.fields[i], fields[i]); + } + dst.fields(std::move(fields)); + dst.is_bigendian(src.is_bigendian); + dst.point_step(src.point_step); + dst.row_step(src.row_step); + dst.data(std::move(src.data)); + dst.is_dense(src.is_dense); +} + +inline void from_fastdds( + const sensor_msgs::msg::PointCloud2& src, + msg::PointCloud2& dst) { + from_fastdds(src.header(), dst.header); + dst.height = src.height(); + dst.width = src.width(); + dst.fields.resize(src.fields().size()); + for (size_t i = 0; i < src.fields().size(); ++i) { + from_fastdds(src.fields()[i], dst.fields[i]); + } + dst.is_bigendian = src.is_bigendian(); + dst.point_step = src.point_step(); + dst.row_step = src.row_step(); + dst.data = src.data(); + dst.is_dense = src.is_dense(); +} + +// --- tf2 messages --- + +inline void to_fastdds(const msg::TFMessage& src, tf2_msgs::msg::TFMessage& dst) { + std::vector transforms(src.transforms.size()); + for (size_t i = 0; i < src.transforms.size(); ++i) { + to_fastdds(src.transforms[i], transforms[i]); + } + dst.transforms(std::move(transforms)); +} + +inline void from_fastdds(const tf2_msgs::msg::TFMessage& src, msg::TFMessage& dst) { + dst.transforms.resize(src.transforms().size()); + for (size_t i = 0; i < src.transforms().size(); ++i) { + from_fastdds(src.transforms()[i], dst.transforms[i]); + } +} + +inline void to_fastdds(const msg::TF2Error& src, tf2_msgs::msg::TF2Error& dst) { + dst.error(src.error); + dst.error_string(src.error_string); +} + +inline void from_fastdds(const tf2_msgs::msg::TF2Error& src, msg::TF2Error& dst) { + dst.error = src.error(); + dst.error_string = src.error_string(); +} + +// --- Navigation --- + +inline void to_fastdds(const msg::Odometry& src, nav_msgs::msg::Odometry& dst) { + to_fastdds(src.header, dst.header()); + dst.child_frame_id(src.child_frame_id); + to_fastdds(src.pose, dst.pose()); + to_fastdds(src.twist, dst.twist()); +} + +inline void from_fastdds(const nav_msgs::msg::Odometry& src, msg::Odometry& dst) { + from_fastdds(src.header(), dst.header); + dst.child_frame_id = src.child_frame_id(); + from_fastdds(src.pose(), dst.pose); + from_fastdds(src.twist(), dst.twist); +} + +// --- Ackermann --- + +inline void to_fastdds( + const msg::AckermannDrive& src, + ackermann_msgs::msg::AckermannDrive& dst) { + dst.steering_angle(src.steering_angle); + dst.steering_angle_velocity(src.steering_angle_velocity); + dst.speed(src.speed); + dst.acceleration(src.acceleration); + dst.jerk(src.jerk); +} + +inline void from_fastdds( + const ackermann_msgs::msg::AckermannDrive& src, + msg::AckermannDrive& dst) { + dst.steering_angle = src.steering_angle(); + dst.steering_angle_velocity = src.steering_angle_velocity(); + dst.speed = src.speed(); + dst.acceleration = src.acceleration(); + dst.jerk = src.jerk(); +} + +inline void to_fastdds( + const msg::AckermannDriveStamped& src, + ackermann_msgs::msg::AckermannDriveStamped& dst) { + to_fastdds(src.header, dst.header()); + to_fastdds(src.drive, dst.drive()); +} + +inline void from_fastdds( + const ackermann_msgs::msg::AckermannDriveStamped& src, + msg::AckermannDriveStamped& dst) { + from_fastdds(src.header(), dst.header); + from_fastdds(src.drive(), dst.drive); +} + +// --- CARLA custom messages --- + +inline void to_fastdds( + const msg::CarlaCollisionEvent& src, + carla_msgs::msg::CarlaCollisionEvent& dst) { + to_fastdds(src.header, dst.header()); + dst.other_actor_id(src.other_actor_id); + to_fastdds(src.normal_impulse, dst.normal_impulse()); +} + +inline void from_fastdds( + const carla_msgs::msg::CarlaCollisionEvent& src, + msg::CarlaCollisionEvent& dst) { + from_fastdds(src.header(), dst.header); + dst.other_actor_id = src.other_actor_id(); + from_fastdds(src.normal_impulse(), dst.normal_impulse); +} + +inline void to_fastdds( + const msg::CarlaEgoVehicleControl& src, + carla_msgs::msg::CarlaEgoVehicleControl& dst) { + to_fastdds(src.header, dst.header()); + dst.throttle(src.throttle); + dst.steer(src.steer); + dst.brake(src.brake); + dst.hand_brake(src.hand_brake); + dst.reverse(src.reverse); + dst.gear(src.gear); + dst.manual_gear_shift(src.manual_gear_shift); +} + +inline void from_fastdds( + const carla_msgs::msg::CarlaEgoVehicleControl& src, + msg::CarlaEgoVehicleControl& dst) { + from_fastdds(src.header(), dst.header); + dst.throttle = src.throttle(); + dst.steer = src.steer(); + dst.brake = src.brake(); + dst.hand_brake = src.hand_brake(); + dst.reverse = src.reverse(); + dst.gear = src.gear(); + dst.manual_gear_shift = src.manual_gear_shift(); +} + +// POD is CarlaLineInvasion, FastDDS class is LaneInvasionEvent +inline void to_fastdds( + const msg::CarlaLineInvasion& src, + carla_msgs::msg::LaneInvasionEvent& dst) { + to_fastdds(src.header, dst.header()); + dst.crossed_lane_markings(src.crossed_lane_markings); +} + +inline void from_fastdds( + const carla_msgs::msg::LaneInvasionEvent& src, + msg::CarlaLineInvasion& dst) { + from_fastdds(src.header(), dst.header); + dst.crossed_lane_markings = src.crossed_lane_markings(); +} + +} // 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 diff --git a/LibCarla/source/test/server/test_dds_middleware.cpp b/LibCarla/source/test/server/test_dds_middleware.cpp new file mode 100644 index 00000000000..a4563b94c81 --- /dev/null +++ b/LibCarla/source/test/server/test_dds_middleware.cpp @@ -0,0 +1,490 @@ +// 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 +// while keeping CARLA_ROS2_DDS_FASTDDS compile-time checks active. +#define CARLA_ROS2_DDS_TESTING +#define CARLA_ROS2_DDS_FASTDDS + +#include "test.h" + +#include +#include +#include +#include +#include +#include + +#include +#include + +using namespace carla::ros2; + +// ========================================================================== +// Test infrastructure +// ========================================================================== + +struct TestMsg { + int value{0}; +}; + +struct TestPubTraits { + using msg_type = TestMsg; +}; + +struct TestSubTraits { + using msg_type = TestMsg; +}; + +// -- Mock publisher middleware ------------------------------------------------ + +class MockPublisherMiddleware : public IDDSPublisherMiddleware { + 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 IDDSSubscriberMiddleware { + 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; } +}; + +// -- Factory fixture (resets static state) ------------------------------------ + +class DDSMiddlewareFactoryFixture : public ::testing::Test { + protected: + void SetUp() override { + DDSMiddlewareFactory::SetMiddleware(DDSMiddleware::FastDDS); + } +}; + +// ========================================================================== +// Group 1: dds_middleware_enum (2 tests) +// ========================================================================== + +TEST(dds_middleware_enum, values_exist) { + DDSMiddleware mw = DDSMiddleware::FastDDS; + EXPECT_EQ(static_cast(mw), 0); +} + +TEST(dds_middleware_enum, switch_covers_all) { + DDSMiddleware mw = DDSMiddleware::FastDDS; + bool covered = false; + switch (mw) { + case DDSMiddleware::FastDDS: + covered = true; + break; + } + EXPECT_TRUE(covered); +} + +// ========================================================================== +// Group 2: dds_middleware_to_string (3 tests) +// ========================================================================== + +TEST(dds_middleware_to_string, fastdds_returns_correct_string) { + EXPECT_STREQ(DDSMiddlewareToString(DDSMiddleware::FastDDS), "FastDDS"); +} + +TEST(dds_middleware_to_string, result_is_not_null) { + const char* result = DDSMiddlewareToString(DDSMiddleware::FastDDS); + ASSERT_NE(result, nullptr); +} + +TEST(dds_middleware_to_string, result_is_not_empty) { + const char* result = DDSMiddlewareToString(DDSMiddleware::FastDDS); + EXPECT_STRNE(result, ""); +} + +// ========================================================================== +// Group 3: dds_middleware_from_string (5 tests) +// ========================================================================== + +TEST(dds_middleware_from_string, fastdds_lowercase_valid) { + auto result = DDSMiddlewareFromString("fastdds"); + EXPECT_TRUE(result.valid); + EXPECT_EQ(result.middleware, DDSMiddleware::FastDDS); +} + +TEST(dds_middleware_from_string, unknown_string_invalid) { + auto result = DDSMiddlewareFromString("cyclonedds"); + EXPECT_FALSE(result.valid); +} + +TEST(dds_middleware_from_string, empty_string_invalid) { + auto result = DDSMiddlewareFromString(""); + EXPECT_FALSE(result.valid); +} + +TEST(dds_middleware_from_string, uppercase_rejected) { + auto result = DDSMiddlewareFromString("FastDDS"); + EXPECT_FALSE(result.valid); +} + +TEST(dds_middleware_from_string, partial_match_rejected) { + auto result = DDSMiddlewareFromString("fast"); + EXPECT_FALSE(result.valid); +} + +// ========================================================================== +// Group 4: dds_middleware_available (2 tests) +// ========================================================================== + +TEST(dds_middleware_available, fastdds_available) { + EXPECT_TRUE( + DDSMiddlewareFactory::IsMiddlewareAvailable(DDSMiddleware::FastDDS)); +} + +TEST(dds_middleware_available, available_string_contains_fastdds) { + std::string available = GetAvailableMiddlewareString(); + EXPECT_NE(available.find("FastDDS"), std::string::npos); +} + +// ========================================================================== +// Group 5: dds_middleware_type_name (4 tests) +// ========================================================================== + +TEST(dds_middleware_type_name, bare_name) { + EXPECT_EQ(ToROS2DDSTypeName("Image"), "dds_::Image_"); +} + +TEST(dds_middleware_type_name, fully_qualified) { + EXPECT_EQ( + ToROS2DDSTypeName("sensor_msgs::msg::Image"), + "sensor_msgs::msg::dds_::Image_"); +} + +TEST(dds_middleware_type_name, single_namespace) { + EXPECT_EQ(ToROS2DDSTypeName("msg::Image"), "msg::dds_::Image_"); +} + +TEST(dds_middleware_type_name, empty_string) { + EXPECT_EQ(ToROS2DDSTypeName(""), "dds_::_"); +} + +// ========================================================================== +// Group 6: DDSMiddlewareFactoryFixture (5 tests) +// ========================================================================== + +TEST_F(DDSMiddlewareFactoryFixture, set_and_get_middleware) { + DDSMiddlewareFactory::SetMiddleware(DDSMiddleware::FastDDS); + EXPECT_EQ(DDSMiddlewareFactory::GetMiddleware(), DDSMiddleware::FastDDS); +} + +TEST_F(DDSMiddlewareFactoryFixture, default_is_fastdds) { + EXPECT_EQ(DDSMiddlewareFactory::GetMiddleware(), DDSMiddleware::FastDDS); +} + +TEST_F(DDSMiddlewareFactoryFixture, is_middleware_available_fastdds) { + EXPECT_TRUE( + DDSMiddlewareFactory::IsMiddlewareAvailable(DDSMiddleware::FastDDS)); +} + +TEST_F(DDSMiddlewareFactoryFixture, resolve_available_middleware) { + auto resolution = + DDSMiddlewareFactory::ResolveMiddleware(DDSMiddleware::FastDDS); + EXPECT_TRUE(resolution.success); + EXPECT_EQ(resolution.middleware, DDSMiddleware::FastDDS); +} + +TEST_F(DDSMiddlewareFactoryFixture, factory_available_string) { + std::string available = DDSMiddlewareFactory::GetAvailableMiddlewareString(); + EXPECT_NE(available.find("FastDDS"), std::string::npos); +} + +// ========================================================================== +// Group 7: dds_publisher_interface (5 tests) +// ========================================================================== + +TEST(dds_publisher_interface, mock_init_success) { + MockPublisherMiddleware mock; + mock.init_return_value = true; + EXPECT_TRUE(mock.Init("rt/test_topic")); + EXPECT_TRUE(mock.init_called); + EXPECT_EQ(mock.last_topic_name, "rt/test_topic"); +} + +TEST(dds_publisher_interface, mock_init_failure) { + MockPublisherMiddleware mock; + mock.init_return_value = false; + EXPECT_FALSE(mock.Init("rt/test_topic")); + EXPECT_TRUE(mock.init_called); +} + +TEST(dds_publisher_interface, mock_publish_records_data) { + MockPublisherMiddleware mock; + TestMsg msg; + msg.value = 42; + EXPECT_TRUE(mock.Publish(&msg)); + EXPECT_TRUE(mock.publish_called); + EXPECT_EQ(mock.last_published_data, &msg); +} + +TEST(dds_publisher_interface, mock_topic_name) { + MockPublisherMiddleware mock; + mock.Init("rt/camera/image"); + EXPECT_EQ(mock.GetTopicName(), "rt/camera/image"); +} + +TEST(dds_publisher_interface, mock_is_alive) { + MockPublisherMiddleware mock; + mock.alive = true; + EXPECT_TRUE(mock.IsAlive()); + mock.alive = false; + EXPECT_FALSE(mock.IsAlive()); +} + +// ========================================================================== +// Group 8: dds_subscriber_interface (5 tests) +// ========================================================================== + +TEST(dds_subscriber_interface, mock_init_stores_pointers) { + MockSubscriberMiddleware mock; + TestMsg msg; + bool flag = false; + EXPECT_TRUE(mock.Init("rt/test_topic", &msg, &flag)); + EXPECT_TRUE(mock.init_called); + EXPECT_EQ(mock.stored_message_ptr, &msg); + EXPECT_EQ(mock.stored_flag_ptr, &flag); +} + +TEST(dds_subscriber_interface, mock_init_failure) { + MockSubscriberMiddleware mock; + mock.init_return_value = false; + TestMsg msg; + bool flag = false; + EXPECT_FALSE(mock.Init("rt/test_topic", &msg, &flag)); +} + +TEST(dds_subscriber_interface, mock_topic_name) { + MockSubscriberMiddleware mock; + TestMsg msg; + bool flag = false; + mock.Init("rt/lidar/points", &msg, &flag); + EXPECT_EQ(mock.GetTopicName(), "rt/lidar/points"); +} + +TEST(dds_subscriber_interface, mock_is_alive) { + MockSubscriberMiddleware mock; + mock.alive = true; + EXPECT_TRUE(mock.IsAlive()); + mock.alive = false; + EXPECT_FALSE(mock.IsAlive()); +} + +TEST(dds_subscriber_interface, mock_simulates_message_receipt) { + MockSubscriberMiddleware mock; + TestMsg msg; + bool flag = false; + mock.Init("rt/test", &msg, &flag); + ASSERT_NE(mock.stored_message_ptr, nullptr); + ASSERT_NE(mock.stored_flag_ptr, nullptr); + auto* typed_ptr = static_cast(mock.stored_message_ptr); + typed_ptr->value = 99; + *mock.stored_flag_ptr = true; + EXPECT_EQ(msg.value, 99); + EXPECT_TRUE(flag); +} + +// ========================================================================== +// Group 9: 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 10: 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")); +} From 98b6f7aa7a7e01ee8c1f95a4c408f16b32c4dd42 Mon Sep 17 00:00:00 2001 From: Jesus Armando Anaya Date: Sun, 29 Mar 2026 23:15:30 -0700 Subject: [PATCH 3/7] feat(LibCarla/ros2): migrate publishers and subscribers to POD message types Switch all concrete publishers and subscribers from FastDDS-generated types to middleware-neutral POD structs (carla::ros2::msg::*). Remove identity TypeMap specializations and generic identity converters from FastDDSTypeMap, leaving only real POD-to-FastDDS mappings. Convert FastDDS accessor syntax (.field(val) / .field()) to direct member access (.field = val / .field). Fix DVS descriptor4 bug where fields were set on descriptor3. Add log_warning on Init failure for all publishers and subscribers. Add missing copyright headers to subscriber CPP files. --- .../carla/ros2/dds/fastdds/FastDDSTypeMap.h | 72 +--------------- .../ros2/publishers/CarlaCameraPublisher.cpp | 54 ++++++------ .../ros2/publishers/CarlaCameraPublisher.h | 20 ++--- .../ros2/publishers/CarlaClockPublisher.cpp | 4 +- .../ros2/publishers/CarlaClockPublisher.h | 10 +-- .../publishers/CarlaCollisionPublisher.cpp | 14 +-- .../ros2/publishers/CarlaCollisionPublisher.h | 10 +-- .../ros2/publishers/CarlaDVSPublisher.cpp | 47 +++++----- .../carla/ros2/publishers/CarlaDVSPublisher.h | 2 +- .../ros2/publishers/CarlaGNSSPublisher.cpp | 12 +-- .../ros2/publishers/CarlaGNSSPublisher.h | 10 +-- .../ros2/publishers/CarlaIMUPublisher.cpp | 26 +++--- .../carla/ros2/publishers/CarlaIMUPublisher.h | 10 +-- .../ros2/publishers/CarlaLidarPublisher.cpp | 50 +++++------ .../ros2/publishers/CarlaLidarPublisher.h | 2 +- .../publishers/CarlaPointCloudPublisher.cpp | 22 ++--- .../publishers/CarlaPointCloudPublisher.h | 12 +-- .../ros2/publishers/CarlaRadarPublisher.cpp | 86 +++++++++---------- .../ros2/publishers/CarlaRadarPublisher.h | 2 +- .../CarlaSemanticLidarPublisher.cpp | 74 ++++++++-------- .../publishers/CarlaSemanticLidarPublisher.h | 2 +- .../publishers/CarlaTransformPublisher.cpp | 33 ++++--- .../ros2/publishers/CarlaTransformPublisher.h | 15 ++-- .../AckermannControlSubscriber.cpp | 14 +-- .../subscribers/AckermannControlSubscriber.h | 11 +-- .../CarlaEgoVehicleControlSubscriber.cpp | 18 ++-- .../CarlaEgoVehicleControlSubscriber.h | 11 +-- 27 files changed, 294 insertions(+), 349 deletions(-) diff --git a/LibCarla/source/carla/ros2/dds/fastdds/FastDDSTypeMap.h b/LibCarla/source/carla/ros2/dds/fastdds/FastDDSTypeMap.h index 1a9ad1935a1..3770477df30 100644 --- a/LibCarla/source/carla/ros2/dds/fastdds/FastDDSTypeMap.h +++ b/LibCarla/source/carla/ros2/dds/fastdds/FastDDSTypeMap.h @@ -47,76 +47,8 @@ namespace ros2 { template struct FastDDSTypeMap; // ============================================================ -// Identity specializations (msg_type IS the FastDDS type) -// Used by current publishers/subscribers until they are migrated -// to POD message types in PR #2b. -// ============================================================ - -template<> struct FastDDSTypeMap { - using fastdds_type = sensor_msgs::msg::NavSatFix; - using fastdds_pubsub_type = sensor_msgs::msg::NavSatFixPubSubType; -}; - -template<> struct FastDDSTypeMap { - using fastdds_type = sensor_msgs::msg::Image; - using fastdds_pubsub_type = sensor_msgs::msg::ImagePubSubType; -}; - -template<> struct FastDDSTypeMap { - using fastdds_type = sensor_msgs::msg::CameraInfo; - using fastdds_pubsub_type = sensor_msgs::msg::CameraInfoPubSubType; -}; - -template<> struct FastDDSTypeMap { - using fastdds_type = sensor_msgs::msg::Imu; - using fastdds_pubsub_type = sensor_msgs::msg::ImuPubSubType; -}; - -template<> struct FastDDSTypeMap { - using fastdds_type = sensor_msgs::msg::PointCloud2; - using fastdds_pubsub_type = sensor_msgs::msg::PointCloud2PubSubType; -}; - -template<> struct FastDDSTypeMap { - using fastdds_type = rosgraph::msg::Clock; - using fastdds_pubsub_type = rosgraph::msg::ClockPubSubType; -}; - -template<> struct FastDDSTypeMap { - using fastdds_type = tf2_msgs::msg::TFMessage; - using fastdds_pubsub_type = tf2_msgs::msg::TFMessagePubSubType; -}; - -template<> struct FastDDSTypeMap { - using fastdds_type = carla_msgs::msg::CarlaCollisionEvent; - using fastdds_pubsub_type = carla_msgs::msg::CarlaCollisionEventPubSubType; -}; - -template<> struct FastDDSTypeMap { - using fastdds_type = carla_msgs::msg::CarlaEgoVehicleControl; - using fastdds_pubsub_type = carla_msgs::msg::CarlaEgoVehicleControlPubSubType; -}; - -template<> struct FastDDSTypeMap { - using fastdds_type = ackermann_msgs::msg::AckermannDriveStamped; - using fastdds_pubsub_type = ackermann_msgs::msg::AckermannDriveStampedPubSubType; -}; - -/// Identity conversion: copy src to dst when both types are the same. -/// Used by publishers/subscribers that have not yet migrated to POD types. -template -inline void to_fastdds(const T& src, T& dst) { - dst = src; -} - -template -inline void from_fastdds(const T& src, T& dst) { - dst = src; -} - -// ============================================================ -// Real specializations (POD msg types -> FastDDS types) -// These map backend-neutral POD structs to FastDDS-generated types. +// Specializations (POD msg types -> FastDDS types) +// These map middleware-neutral POD structs to FastDDS-generated types. // Conversion functions are in types/FastDDSConversions.h. // ============================================================ diff --git a/LibCarla/source/carla/ros2/publishers/CarlaCameraPublisher.cpp b/LibCarla/source/carla/ros2/publishers/CarlaCameraPublisher.cpp index edb98ff4eae..cb339516b2b 100644 --- a/LibCarla/source/carla/ros2/publishers/CarlaCameraPublisher.cpp +++ b/LibCarla/source/carla/ros2/publishers/CarlaCameraPublisher.cpp @@ -15,46 +15,46 @@ std::vector CarlaCameraPublisher::ComputeImage(uint32_t height, uint32_ bool CarlaCameraPublisher::WriteCameraInfo(int32_t seconds, uint32_t nanoseconds, uint32_t x_offset, uint32_t y_offset, uint32_t height, uint32_t width, float fov, bool do_rectify) { - _impl_camera_info->GetMessage()->header().stamp().sec(seconds); - _impl_camera_info->GetMessage()->header().stamp().nanosec(nanoseconds); - _impl_camera_info->GetMessage()->header().frame_id(GetFrameId()); + _impl_camera_info->GetMessage()->header.stamp.sec = seconds; + _impl_camera_info->GetMessage()->header.stamp.nanosec = nanoseconds; + _impl_camera_info->GetMessage()->header.frame_id = GetFrameId(); const double cx = static_cast(width) / 2.0; const double cy = static_cast(height) / 2.0; const double fx = static_cast(width) / (2.0 * std::tan(fov * M_PI / 360.0)); const double fy = fx; - _impl_camera_info->GetMessage()->height(height); - _impl_camera_info->GetMessage()->width(width); - _impl_camera_info->GetMessage()->distortion_model("plumb_bob"); - _impl_camera_info->GetMessage()->D({ 0.0, 0.0, 0.0, 0.0, 0.0 }); - _impl_camera_info->GetMessage()->k({fx, 0.0, cx, 0.0, fy, cy, 0.0, 0.0, 1.0}); - _impl_camera_info->GetMessage()->r({ 1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0 }); - _impl_camera_info->GetMessage()->p({fx, 0.0, cx, 0.0, 0.0, fy, cy, 0.0, 0.0, 0.0, 1.0, 0.0}); - _impl_camera_info->GetMessage()->binning_x(0); - _impl_camera_info->GetMessage()->binning_y(0); - - _impl_camera_info->GetMessage()->roi().x_offset(x_offset); - _impl_camera_info->GetMessage()->roi().y_offset(y_offset); - _impl_camera_info->GetMessage()->roi().height(height); - _impl_camera_info->GetMessage()->roi().width(width); - _impl_camera_info->GetMessage()->roi().do_rectify(do_rectify); + _impl_camera_info->GetMessage()->height = height; + _impl_camera_info->GetMessage()->width = width; + _impl_camera_info->GetMessage()->distortion_model = "plumb_bob"; + _impl_camera_info->GetMessage()->d = { 0.0, 0.0, 0.0, 0.0, 0.0 }; + _impl_camera_info->GetMessage()->k = {fx, 0.0, cx, 0.0, fy, cy, 0.0, 0.0, 1.0}; + _impl_camera_info->GetMessage()->r = { 1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0 }; + _impl_camera_info->GetMessage()->p = {fx, 0.0, cx, 0.0, 0.0, fy, cy, 0.0, 0.0, 0.0, 1.0, 0.0}; + _impl_camera_info->GetMessage()->binning_x = 0; + _impl_camera_info->GetMessage()->binning_y = 0; + + _impl_camera_info->GetMessage()->roi.x_offset = x_offset; + _impl_camera_info->GetMessage()->roi.y_offset = y_offset; + _impl_camera_info->GetMessage()->roi.height = height; + _impl_camera_info->GetMessage()->roi.width = width; + _impl_camera_info->GetMessage()->roi.do_rectify = do_rectify; return true; } bool CarlaCameraPublisher::WriteImage(int32_t seconds, uint32_t nanoseconds, uint32_t height, uint32_t width, std::vector data) { - _impl_image->GetMessage()->header().stamp().sec(seconds); - _impl_image->GetMessage()->header().stamp().nanosec(nanoseconds); - _impl_image->GetMessage()->header().frame_id(this->GetFrameId()); + _impl_image->GetMessage()->header.stamp.sec = seconds; + _impl_image->GetMessage()->header.stamp.nanosec = nanoseconds; + _impl_image->GetMessage()->header.frame_id = this->GetFrameId(); - _impl_image->GetMessage()->width(width); - _impl_image->GetMessage()->height(height); - _impl_image->GetMessage()->encoding(this->GetEncoding()); - _impl_image->GetMessage()->is_bigendian(0); - _impl_image->GetMessage()->step(width * this->GetChannels() * sizeof(uint8_t)); + _impl_image->GetMessage()->width = width; + _impl_image->GetMessage()->height = height; + _impl_image->GetMessage()->encoding = this->GetEncoding(); + _impl_image->GetMessage()->is_bigendian = 0; + _impl_image->GetMessage()->step = width * this->GetChannels() * sizeof(uint8_t); - _impl_image->GetMessage()->data(std::move(data)); // https://github.com/eProsima/Fast-DDS/issues/2330 + _impl_image->GetMessage()->data = std::move(data); return true; } diff --git a/LibCarla/source/carla/ros2/publishers/CarlaCameraPublisher.h b/LibCarla/source/carla/ros2/publishers/CarlaCameraPublisher.h index 523b152a5c7..e3870c34443 100644 --- a/LibCarla/source/carla/ros2/publishers/CarlaCameraPublisher.h +++ b/LibCarla/source/carla/ros2/publishers/CarlaCameraPublisher.h @@ -10,10 +10,8 @@ #include "carla/ros2/publishers/BasePublisher.h" #include "carla/ros2/publishers/PublisherImpl.h" -#include "carla/ros2/types/Image.h" -#include "carla/ros2/types/ImagePubSubTypes.h" -#include "carla/ros2/types/CameraInfo.h" -#include "carla/ros2/types/CameraInfoPubSubTypes.h" +#include "carla/ros2/types/msg/Image.h" +#include "carla/ros2/types/msg/CameraInfo.h" namespace carla { namespace ros2 { @@ -21,21 +19,23 @@ namespace ros2 { class CarlaCameraPublisher : public BasePublisher { public: struct ImageMsgTraits { - using msg_type = sensor_msgs::msg::Image; - using msg_pubsub_type = sensor_msgs::msg::ImagePubSubType; + using msg_type = msg::Image; }; struct CameraInfoMsgTraits { - using msg_type = sensor_msgs::msg::CameraInfo; - using msg_pubsub_type = sensor_msgs::msg::CameraInfoPubSubType; + using msg_type = msg::CameraInfo; }; CarlaCameraPublisher(std::string base_topic_name, std::string frame_id) : BasePublisher(base_topic_name, frame_id), _impl_image(std::make_shared>()), _impl_camera_info(std::make_shared>()) { - _impl_image->Init(GetBaseTopicName() + "/image"); - _impl_camera_info->Init(GetBaseTopicName() + "/camera_info"); + if (!_impl_image->Init(GetBaseTopicName() + "/image")) { + log_warning("CarlaCameraPublisher: Init failed for topic: ", GetBaseTopicName(), "/image"); + } + if (!_impl_camera_info->Init(GetBaseTopicName() + "/camera_info")) { + log_warning("CarlaCameraPublisher: Init failed for topic: ", GetBaseTopicName(), "/camera_info"); + } } virtual uint8_t GetChannels() = 0; diff --git a/LibCarla/source/carla/ros2/publishers/CarlaClockPublisher.cpp b/LibCarla/source/carla/ros2/publishers/CarlaClockPublisher.cpp index 2f287c25776..dbb42be55f7 100644 --- a/LibCarla/source/carla/ros2/publishers/CarlaClockPublisher.cpp +++ b/LibCarla/source/carla/ros2/publishers/CarlaClockPublisher.cpp @@ -8,8 +8,8 @@ namespace carla { namespace ros2 { bool CarlaClockPublisher::Write(int32_t seconds, uint32_t nanoseconds) { - _impl->GetMessage()->clock().sec(seconds); - _impl->GetMessage()->clock().nanosec(nanoseconds); + _impl->GetMessage()->clock.sec = seconds; + _impl->GetMessage()->clock.nanosec = nanoseconds; return true; } diff --git a/LibCarla/source/carla/ros2/publishers/CarlaClockPublisher.h b/LibCarla/source/carla/ros2/publishers/CarlaClockPublisher.h index 85dca7e225f..731e584a524 100644 --- a/LibCarla/source/carla/ros2/publishers/CarlaClockPublisher.h +++ b/LibCarla/source/carla/ros2/publishers/CarlaClockPublisher.h @@ -9,8 +9,7 @@ #include "carla/ros2/publishers/BasePublisher.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 { @@ -18,14 +17,15 @@ namespace ros2 { class CarlaClockPublisher : public BasePublisher { public: struct ClockMsgTraits { - using msg_type = rosgraph::msg::Clock; - using msg_pubsub_type = rosgraph::msg::ClockPubSubType; + using msg_type = msg::Clock; }; CarlaClockPublisher() : BasePublisher("rt/clock"), _impl(std::make_shared>()) { - _impl->Init(GetBaseTopicName()); + if (!_impl->Init(GetBaseTopicName())) { + log_warning("CarlaClockPublisher: Init failed for topic: ", GetBaseTopicName()); + } } bool Publish() { diff --git a/LibCarla/source/carla/ros2/publishers/CarlaCollisionPublisher.cpp b/LibCarla/source/carla/ros2/publishers/CarlaCollisionPublisher.cpp index c5879c9d7ed..f75341012ba 100644 --- a/LibCarla/source/carla/ros2/publishers/CarlaCollisionPublisher.cpp +++ b/LibCarla/source/carla/ros2/publishers/CarlaCollisionPublisher.cpp @@ -9,15 +9,15 @@ namespace ros2 { bool CarlaCollisionPublisher::Write(int32_t seconds, uint32_t nanoseconds, uint32_t actor_id, geom::Vector3D impulse) { - _impl->GetMessage()->header().stamp().sec(seconds); - _impl->GetMessage()->header().stamp().nanosec(nanoseconds); - _impl->GetMessage()->header().frame_id(GetFrameId()); + _impl->GetMessage()->header.stamp.sec = seconds; + _impl->GetMessage()->header.stamp.nanosec = nanoseconds; + _impl->GetMessage()->header.frame_id = GetFrameId(); - _impl->GetMessage()->other_actor_id(actor_id); + _impl->GetMessage()->other_actor_id = actor_id; - _impl->GetMessage()->normal_impulse().x(impulse.x); - _impl->GetMessage()->normal_impulse().y(impulse.y); - _impl->GetMessage()->normal_impulse().z(impulse.z); + _impl->GetMessage()->normal_impulse.x = impulse.x; + _impl->GetMessage()->normal_impulse.y = impulse.y; + _impl->GetMessage()->normal_impulse.z = impulse.z; return true; } diff --git a/LibCarla/source/carla/ros2/publishers/CarlaCollisionPublisher.h b/LibCarla/source/carla/ros2/publishers/CarlaCollisionPublisher.h index 081eda3dfe4..53d2d8649d0 100644 --- a/LibCarla/source/carla/ros2/publishers/CarlaCollisionPublisher.h +++ b/LibCarla/source/carla/ros2/publishers/CarlaCollisionPublisher.h @@ -11,8 +11,7 @@ #include "carla/ros2/publishers/BasePublisher.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 { @@ -20,14 +19,15 @@ namespace ros2 { class CarlaCollisionPublisher : public BasePublisher { public: struct CollisionMsgTraits { - using msg_type = carla_msgs::msg::CarlaCollisionEvent; - using msg_pubsub_type = carla_msgs::msg::CarlaCollisionEventPubSubType; + using msg_type = msg::CarlaCollisionEvent; }; CarlaCollisionPublisher(std::string base_topic_name, std::string frame_id) : BasePublisher(base_topic_name, frame_id), _impl(std::make_shared>()) { - _impl->Init(this->GetBaseTopicName()); + if (!_impl->Init(this->GetBaseTopicName())) { + log_warning("CarlaCollisionPublisher: Init failed for topic: ", this->GetBaseTopicName()); + } } bool Publish() { diff --git a/LibCarla/source/carla/ros2/publishers/CarlaDVSPublisher.cpp b/LibCarla/source/carla/ros2/publishers/CarlaDVSPublisher.cpp index d47d624dc1c..ed8e11911b6 100644 --- a/LibCarla/source/carla/ros2/publishers/CarlaDVSPublisher.cpp +++ b/LibCarla/source/carla/ros2/publishers/CarlaDVSPublisher.cpp @@ -13,28 +13,31 @@ size_t CarlaDVSPointCloudPublisher::GetPointSize() { return sizeof(sensor::data::DVSEvent); } -std::vector CarlaDVSPointCloudPublisher::GetFields() { - - sensor_msgs::msg::PointField descriptor1; - descriptor1.name("x"); - descriptor1.offset(0); - descriptor1.datatype(sensor_msgs::msg::PointField__UINT16); - descriptor1.count(1); - sensor_msgs::msg::PointField descriptor2; - descriptor2.name("y"); - descriptor2.offset(2); - descriptor2.datatype(sensor_msgs::msg::PointField__UINT16); - descriptor2.count(1); - sensor_msgs::msg::PointField descriptor3; - descriptor3.name("t"); - descriptor3.offset(4); - descriptor3.datatype(sensor_msgs::msg::PointField__FLOAT64); - descriptor3.count(1); - sensor_msgs::msg::PointField descriptor4; - descriptor3.name("pol"); - descriptor3.offset(12); - descriptor3.datatype(sensor_msgs::msg::PointField__INT8); - descriptor3.count(1); +std::vector CarlaDVSPointCloudPublisher::GetFields() { + + msg::PointField descriptor1; + descriptor1.name = "x"; + descriptor1.offset = 0; + descriptor1.datatype = msg::PointField::UINT16; + descriptor1.count = 1; + + msg::PointField descriptor2; + descriptor2.name = "y"; + descriptor2.offset = 2; + descriptor2.datatype = msg::PointField::UINT16; + descriptor2.count = 1; + + msg::PointField descriptor3; + descriptor3.name = "t"; + descriptor3.offset = 4; + descriptor3.datatype = msg::PointField::FLOAT64; + descriptor3.count = 1; + + msg::PointField descriptor4; + descriptor4.name = "pol"; + descriptor4.offset = 12; + descriptor4.datatype = msg::PointField::INT8; + descriptor4.count = 1; return {descriptor1, descriptor2, descriptor3, descriptor4}; } diff --git a/LibCarla/source/carla/ros2/publishers/CarlaDVSPublisher.h b/LibCarla/source/carla/ros2/publishers/CarlaDVSPublisher.h index 0242ac96d6c..f4e6f5b8261 100644 --- a/LibCarla/source/carla/ros2/publishers/CarlaDVSPublisher.h +++ b/LibCarla/source/carla/ros2/publishers/CarlaDVSPublisher.h @@ -36,7 +36,7 @@ namespace ros2 { private: size_t GetPointSize() override; - std::vector GetFields() override; + std::vector GetFields() override; std::vector ComputePointCloud(uint32_t height, uint32_t width, uint8_t *data) override; }; diff --git a/LibCarla/source/carla/ros2/publishers/CarlaGNSSPublisher.cpp b/LibCarla/source/carla/ros2/publishers/CarlaGNSSPublisher.cpp index bb79bb56fa8..7309d6c609b 100644 --- a/LibCarla/source/carla/ros2/publishers/CarlaGNSSPublisher.cpp +++ b/LibCarla/source/carla/ros2/publishers/CarlaGNSSPublisher.cpp @@ -9,13 +9,13 @@ namespace ros2 { bool CarlaGNSSPublisher::Write(int32_t seconds, uint32_t nanoseconds, const geom::GeoLocation data) { - _impl->GetMessage()->header().stamp().sec(seconds); - _impl->GetMessage()->header().stamp().nanosec(nanoseconds); - _impl->GetMessage()->header().frame_id(GetFrameId()); + _impl->GetMessage()->header.stamp.sec = seconds; + _impl->GetMessage()->header.stamp.nanosec = nanoseconds; + _impl->GetMessage()->header.frame_id = GetFrameId(); - _impl->GetMessage()->latitude(data.latitude); - _impl->GetMessage()->longitude(data.longitude); - _impl->GetMessage()->altitude(data.altitude); + _impl->GetMessage()->latitude = data.latitude; + _impl->GetMessage()->longitude = data.longitude; + _impl->GetMessage()->altitude = data.altitude; return true; } diff --git a/LibCarla/source/carla/ros2/publishers/CarlaGNSSPublisher.h b/LibCarla/source/carla/ros2/publishers/CarlaGNSSPublisher.h index abcc0217ac6..4e5812739d3 100644 --- a/LibCarla/source/carla/ros2/publishers/CarlaGNSSPublisher.h +++ b/LibCarla/source/carla/ros2/publishers/CarlaGNSSPublisher.h @@ -11,8 +11,7 @@ #include "carla/ros2/publishers/BasePublisher.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 { @@ -20,14 +19,15 @@ namespace ros2 { class CarlaGNSSPublisher : public BasePublisher { public: struct GnssMsgTraits { - using msg_type = sensor_msgs::msg::NavSatFix; - using msg_pubsub_type = sensor_msgs::msg::NavSatFixPubSubType; + using msg_type = msg::NavSatFix; }; CarlaGNSSPublisher(std::string base_topic_name, std::string frame_id): BasePublisher(base_topic_name, frame_id), _impl(std::make_shared>()) { - _impl->Init(this->GetBaseTopicName()); + if (!_impl->Init(this->GetBaseTopicName())) { + log_warning("CarlaGNSSPublisher: Init failed for topic: ", this->GetBaseTopicName()); + } } bool Publish() { diff --git a/LibCarla/source/carla/ros2/publishers/CarlaIMUPublisher.cpp b/LibCarla/source/carla/ros2/publishers/CarlaIMUPublisher.cpp index 3a2afa170b7..cf0d0a3cbca 100644 --- a/LibCarla/source/carla/ros2/publishers/CarlaIMUPublisher.cpp +++ b/LibCarla/source/carla/ros2/publishers/CarlaIMUPublisher.cpp @@ -9,17 +9,17 @@ namespace ros2 { bool CarlaIMUPublisher::Write(int32_t seconds, uint32_t nanoseconds, geom::Vector3D accelerometer, geom::Vector3D gyroscope, float compass) { - _impl->GetMessage()->header().stamp().sec(seconds); - _impl->GetMessage()->header().stamp().nanosec(nanoseconds); - _impl->GetMessage()->header().frame_id(GetFrameId()); + _impl->GetMessage()->header.stamp.sec = seconds; + _impl->GetMessage()->header.stamp.nanosec = nanoseconds; + _impl->GetMessage()->header.frame_id = GetFrameId(); - _impl->GetMessage()->linear_acceleration().x(accelerometer.x); - _impl->GetMessage()->linear_acceleration().y(-accelerometer.y); - _impl->GetMessage()->linear_acceleration().z(accelerometer.z); + _impl->GetMessage()->linear_acceleration.x = accelerometer.x; + _impl->GetMessage()->linear_acceleration.y = -accelerometer.y; + _impl->GetMessage()->linear_acceleration.z = accelerometer.z; - _impl->GetMessage()->angular_velocity().x(-gyroscope.x); - _impl->GetMessage()->angular_velocity().y(gyroscope.y); - _impl->GetMessage()->angular_velocity().z(-gyroscope.z); + _impl->GetMessage()->angular_velocity.x = -gyroscope.x; + _impl->GetMessage()->angular_velocity.y = gyroscope.y; + _impl->GetMessage()->angular_velocity.z = -gyroscope.z; const float rx = 0.0f; // pitch const float ry = (float(M_PI_2) / 2.0f) - compass; // yaw @@ -37,10 +37,10 @@ bool CarlaIMUPublisher::Write(int32_t seconds, uint32_t nanoseconds, geom::Vecto const float y = cr * sp * cy + sr * cp * sy; const float z = cr * cp * sy - sr * sp * cy; - _impl->GetMessage()->orientation().w(w); - _impl->GetMessage()->orientation().x(x); - _impl->GetMessage()->orientation().y(y); - _impl->GetMessage()->orientation().z(z); + _impl->GetMessage()->orientation.w = w; + _impl->GetMessage()->orientation.x = x; + _impl->GetMessage()->orientation.y = y; + _impl->GetMessage()->orientation.z = z; return true; } diff --git a/LibCarla/source/carla/ros2/publishers/CarlaIMUPublisher.h b/LibCarla/source/carla/ros2/publishers/CarlaIMUPublisher.h index e9d70472ed3..082a096679b 100644 --- a/LibCarla/source/carla/ros2/publishers/CarlaIMUPublisher.h +++ b/LibCarla/source/carla/ros2/publishers/CarlaIMUPublisher.h @@ -11,8 +11,7 @@ #include "carla/ros2/publishers/BasePublisher.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 { @@ -20,14 +19,15 @@ namespace ros2 { class CarlaIMUPublisher : public BasePublisher { public: struct ImuMsgTraits { - using msg_type = sensor_msgs::msg::Imu; - using msg_pubsub_type = sensor_msgs::msg::ImuPubSubType; + using msg_type = msg::Imu; }; CarlaIMUPublisher(std::string base_topic_name, std::string frame_id) : BasePublisher(base_topic_name, frame_id), _impl(std::make_shared>()) { - _impl->Init(this->GetBaseTopicName()); + if (!_impl->Init(this->GetBaseTopicName())) { + log_warning("CarlaIMUPublisher: Init failed for topic: ", this->GetBaseTopicName()); + } } bool Publish() { diff --git a/LibCarla/source/carla/ros2/publishers/CarlaLidarPublisher.cpp b/LibCarla/source/carla/ros2/publishers/CarlaLidarPublisher.cpp index d080de4f2e5..c46665cb970 100644 --- a/LibCarla/source/carla/ros2/publishers/CarlaLidarPublisher.cpp +++ b/LibCarla/source/carla/ros2/publishers/CarlaLidarPublisher.cpp @@ -13,31 +13,31 @@ size_t CarlaLidarPublisher::GetPointSize() { return sizeof(sensor::data::LidarDetection); } -std::vector CarlaLidarPublisher::GetFields() { - - sensor_msgs::msg::PointField descriptor1; - descriptor1.name("x"); - descriptor1.offset(0); - descriptor1.datatype(sensor_msgs::msg::PointField__FLOAT32); - descriptor1.count(1); - - sensor_msgs::msg::PointField descriptor2; - descriptor2.name("y"); - descriptor2.offset(4); - descriptor2.datatype(sensor_msgs::msg::PointField__FLOAT32); - descriptor2.count(1); - - sensor_msgs::msg::PointField descriptor3; - descriptor3.name("z"); - descriptor3.offset(8); - descriptor3.datatype(sensor_msgs::msg::PointField__FLOAT32); - descriptor3.count(1); - - sensor_msgs::msg::PointField descriptor4; - descriptor4.name("intensity"); - descriptor4.offset(12); - descriptor4.datatype(sensor_msgs::msg::PointField__FLOAT32); - descriptor4.count(1); +std::vector CarlaLidarPublisher::GetFields() { + + msg::PointField descriptor1; + descriptor1.name = "x"; + descriptor1.offset = 0; + descriptor1.datatype = msg::PointField::FLOAT32; + descriptor1.count = 1; + + msg::PointField descriptor2; + descriptor2.name = "y"; + descriptor2.offset = 4; + descriptor2.datatype = msg::PointField::FLOAT32; + descriptor2.count = 1; + + msg::PointField descriptor3; + descriptor3.name = "z"; + descriptor3.offset = 8; + descriptor3.datatype = msg::PointField::FLOAT32; + descriptor3.count = 1; + + msg::PointField descriptor4; + descriptor4.name = "intensity"; + descriptor4.offset = 12; + descriptor4.datatype = msg::PointField::FLOAT32; + descriptor4.count = 1; return {descriptor1, descriptor2, descriptor3, descriptor4}; } diff --git a/LibCarla/source/carla/ros2/publishers/CarlaLidarPublisher.h b/LibCarla/source/carla/ros2/publishers/CarlaLidarPublisher.h index e35d59cc550..bc13a5141dc 100644 --- a/LibCarla/source/carla/ros2/publishers/CarlaLidarPublisher.h +++ b/LibCarla/source/carla/ros2/publishers/CarlaLidarPublisher.h @@ -18,7 +18,7 @@ namespace ros2 { private: size_t GetPointSize() override; - std::vector GetFields() override; + std::vector GetFields() override; std::vector ComputePointCloud(uint32_t height, uint32_t width, uint8_t *data) override; }; diff --git a/LibCarla/source/carla/ros2/publishers/CarlaPointCloudPublisher.cpp b/LibCarla/source/carla/ros2/publishers/CarlaPointCloudPublisher.cpp index 7801657ea9c..cf46987737a 100644 --- a/LibCarla/source/carla/ros2/publishers/CarlaPointCloudPublisher.cpp +++ b/LibCarla/source/carla/ros2/publishers/CarlaPointCloudPublisher.cpp @@ -9,21 +9,21 @@ namespace ros2 { bool CarlaPointCloudPublisher::WritePointCloud(int32_t seconds, uint32_t nanoseconds, uint32_t height, uint32_t width, std::vector data) { - _impl->GetMessage()->header().stamp().sec(seconds); - _impl->GetMessage()->header().stamp().nanosec(nanoseconds); - _impl->GetMessage()->header().frame_id(GetFrameId()); + _impl->GetMessage()->header.stamp.sec = seconds; + _impl->GetMessage()->header.stamp.nanosec = nanoseconds; + _impl->GetMessage()->header.frame_id = GetFrameId(); auto fields = GetFields(); const size_t point_size = GetPointSize(); - _impl->GetMessage()->width(width); - _impl->GetMessage()->height(height); - _impl->GetMessage()->is_bigendian(false); - _impl->GetMessage()->fields(fields); - _impl->GetMessage()->point_step(static_cast(point_size)); - _impl->GetMessage()->row_step(static_cast(width * point_size)); - _impl->GetMessage()->is_dense(false); // True if there are not invalid points - _impl->GetMessage()->data(std::move(data)); + _impl->GetMessage()->width = width; + _impl->GetMessage()->height = height; + _impl->GetMessage()->is_bigendian = false; + _impl->GetMessage()->fields = std::move(fields); + _impl->GetMessage()->point_step = static_cast(point_size); + _impl->GetMessage()->row_step = width * static_cast(point_size); + _impl->GetMessage()->is_dense = false; + _impl->GetMessage()->data = std::move(data); return true; } diff --git a/LibCarla/source/carla/ros2/publishers/CarlaPointCloudPublisher.h b/LibCarla/source/carla/ros2/publishers/CarlaPointCloudPublisher.h index 6148f727090..64cebb4f242 100644 --- a/LibCarla/source/carla/ros2/publishers/CarlaPointCloudPublisher.h +++ b/LibCarla/source/carla/ros2/publishers/CarlaPointCloudPublisher.h @@ -10,8 +10,7 @@ #include "carla/ros2/publishers/BasePublisher.h" #include "carla/ros2/publishers/PublisherImpl.h" -#include "carla/ros2/types/PointCloud2.h" -#include "carla/ros2/types/PointCloud2PubSubTypes.h" +#include "carla/ros2/types/msg/PointCloud2.h" namespace carla { namespace ros2 { @@ -19,14 +18,15 @@ namespace ros2 { class CarlaPointCloudPublisher : public BasePublisher { public: struct PointCloudMsgTraits { - using msg_type = sensor_msgs::msg::PointCloud2; - using msg_pubsub_type = sensor_msgs::msg::PointCloud2PubSubType; + using msg_type = msg::PointCloud2; }; CarlaPointCloudPublisher(std::string base_topic_name, std::string frame_id) : BasePublisher(base_topic_name, frame_id), _impl(std::make_shared>()) { - _impl->Init(GetBaseTopicName() + "/point_cloud"); + if (!_impl->Init(GetBaseTopicName() + "/point_cloud")) { + log_warning("CarlaPointCloudPublisher: Init failed for topic: ", GetBaseTopicName(), "/point_cloud"); + } } bool Publish() { @@ -40,7 +40,7 @@ namespace ros2 { private: virtual size_t GetPointSize() = 0; - virtual std::vector GetFields() = 0; + virtual std::vector GetFields() = 0; virtual std::vector ComputePointCloud(uint32_t height, uint32_t width, uint8_t *data) = 0; diff --git a/LibCarla/source/carla/ros2/publishers/CarlaRadarPublisher.cpp b/LibCarla/source/carla/ros2/publishers/CarlaRadarPublisher.cpp index aceb1f6648a..b848197f4b5 100644 --- a/LibCarla/source/carla/ros2/publishers/CarlaRadarPublisher.cpp +++ b/LibCarla/source/carla/ros2/publishers/CarlaRadarPublisher.cpp @@ -20,49 +20,49 @@ size_t CarlaRadarPublisher::GetPointSize() { return sizeof(RadarDetectionWithPosition); } -std::vector CarlaRadarPublisher::GetFields() { - - sensor_msgs::msg::PointField descriptor1; - descriptor1.name("x"); - descriptor1.offset(0); - descriptor1.datatype(sensor_msgs::msg::PointField__FLOAT32); - descriptor1.count(1); - - sensor_msgs::msg::PointField descriptor2; - descriptor2.name("y"); - descriptor2.offset(4); - descriptor2.datatype(sensor_msgs::msg::PointField__FLOAT32); - descriptor2.count(1); - - sensor_msgs::msg::PointField descriptor3; - descriptor3.name("z"); - descriptor3.offset(8); - descriptor3.datatype(sensor_msgs::msg::PointField__FLOAT32); - descriptor3.count(1); - - sensor_msgs::msg::PointField descriptor4; - descriptor4.name("velocity"); - descriptor4.offset(12); - descriptor4.datatype(sensor_msgs::msg::PointField__FLOAT32); - descriptor4.count(1); - - sensor_msgs::msg::PointField descriptor5; - descriptor5.name("azimuth"); - descriptor5.offset(16); - descriptor5.datatype(sensor_msgs::msg::PointField__FLOAT32); - descriptor5.count(1); - - sensor_msgs::msg::PointField descriptor6; - descriptor6.name("altitude"); - descriptor6.offset(20); - descriptor6.datatype(sensor_msgs::msg::PointField__FLOAT32); - descriptor6.count(1); - - sensor_msgs::msg::PointField descriptor7; - descriptor7.name("depth"); - descriptor7.offset(24); - descriptor7.datatype(sensor_msgs::msg::PointField__FLOAT32); - descriptor7.count(1); +std::vector CarlaRadarPublisher::GetFields() { + + msg::PointField descriptor1; + descriptor1.name = "x"; + descriptor1.offset = 0; + descriptor1.datatype = msg::PointField::FLOAT32; + descriptor1.count = 1; + + msg::PointField descriptor2; + descriptor2.name = "y"; + descriptor2.offset = 4; + descriptor2.datatype = msg::PointField::FLOAT32; + descriptor2.count = 1; + + msg::PointField descriptor3; + descriptor3.name = "z"; + descriptor3.offset = 8; + descriptor3.datatype = msg::PointField::FLOAT32; + descriptor3.count = 1; + + msg::PointField descriptor4; + descriptor4.name = "velocity"; + descriptor4.offset = 12; + descriptor4.datatype = msg::PointField::FLOAT32; + descriptor4.count = 1; + + msg::PointField descriptor5; + descriptor5.name = "azimuth"; + descriptor5.offset = 16; + descriptor5.datatype = msg::PointField::FLOAT32; + descriptor5.count = 1; + + msg::PointField descriptor6; + descriptor6.name = "altitude"; + descriptor6.offset = 20; + descriptor6.datatype = msg::PointField::FLOAT32; + descriptor6.count = 1; + + msg::PointField descriptor7; + descriptor7.name = "depth"; + descriptor7.offset = 24; + descriptor7.datatype = msg::PointField::FLOAT32; + descriptor7.count = 1; return {descriptor1, descriptor2, descriptor3, descriptor4, descriptor5, descriptor6, descriptor7}; } diff --git a/LibCarla/source/carla/ros2/publishers/CarlaRadarPublisher.h b/LibCarla/source/carla/ros2/publishers/CarlaRadarPublisher.h index a7fe60511b1..a8c90b05338 100644 --- a/LibCarla/source/carla/ros2/publishers/CarlaRadarPublisher.h +++ b/LibCarla/source/carla/ros2/publishers/CarlaRadarPublisher.h @@ -19,7 +19,7 @@ namespace ros2 { private: size_t GetPointSize() override; - std::vector GetFields() override; + std::vector GetFields() override; std::vector ComputePointCloud(uint32_t height, uint32_t width, uint8_t *data) override; }; diff --git a/LibCarla/source/carla/ros2/publishers/CarlaSemanticLidarPublisher.cpp b/LibCarla/source/carla/ros2/publishers/CarlaSemanticLidarPublisher.cpp index 8a87193d840..d26b0ef6314 100644 --- a/LibCarla/source/carla/ros2/publishers/CarlaSemanticLidarPublisher.cpp +++ b/LibCarla/source/carla/ros2/publishers/CarlaSemanticLidarPublisher.cpp @@ -13,43 +13,43 @@ size_t CarlaSemanticLidarPublisher::GetPointSize() { return sizeof(sensor::data::SemanticLidarDetection); } -std::vector CarlaSemanticLidarPublisher::GetFields() { - - sensor_msgs::msg::PointField descriptor1; - descriptor1.name("x"); - descriptor1.offset(0); - descriptor1.datatype(sensor_msgs::msg::PointField__FLOAT32); - descriptor1.count(1); - - sensor_msgs::msg::PointField descriptor2; - descriptor2.name("y"); - descriptor2.offset(4); - descriptor2.datatype(sensor_msgs::msg::PointField__FLOAT32); - descriptor2.count(1); - - sensor_msgs::msg::PointField descriptor3; - descriptor3.name("z"); - descriptor3.offset(8); - descriptor3.datatype(sensor_msgs::msg::PointField__FLOAT32); - descriptor3.count(1); - - sensor_msgs::msg::PointField descriptor4; - descriptor4.name("cos_inc_angle"); - descriptor4.offset(12); - descriptor4.datatype(sensor_msgs::msg::PointField__FLOAT32); - descriptor4.count(1); - - sensor_msgs::msg::PointField descriptor5; - descriptor5.name("object_idx"); - descriptor5.offset(16); - descriptor5.datatype(sensor_msgs::msg::PointField__UINT32); - descriptor5.count(1); - - sensor_msgs::msg::PointField descriptor6; - descriptor6.name("object_tag"); - descriptor6.offset(20); - descriptor6.datatype(sensor_msgs::msg::PointField__UINT32); - descriptor6.count(1); +std::vector CarlaSemanticLidarPublisher::GetFields() { + + msg::PointField descriptor1; + descriptor1.name = "x"; + descriptor1.offset = 0; + descriptor1.datatype = msg::PointField::FLOAT32; + descriptor1.count = 1; + + msg::PointField descriptor2; + descriptor2.name = "y"; + descriptor2.offset = 4; + descriptor2.datatype = msg::PointField::FLOAT32; + descriptor2.count = 1; + + msg::PointField descriptor3; + descriptor3.name = "z"; + descriptor3.offset = 8; + descriptor3.datatype = msg::PointField::FLOAT32; + descriptor3.count = 1; + + msg::PointField descriptor4; + descriptor4.name = "cos_inc_angle"; + descriptor4.offset = 12; + descriptor4.datatype = msg::PointField::FLOAT32; + descriptor4.count = 1; + + msg::PointField descriptor5; + descriptor5.name = "object_idx"; + descriptor5.offset = 16; + descriptor5.datatype = msg::PointField::UINT32; + descriptor5.count = 1; + + msg::PointField descriptor6; + descriptor6.name = "object_tag"; + descriptor6.offset = 20; + descriptor6.datatype = msg::PointField::UINT32; + descriptor6.count = 1; return {descriptor1, descriptor2, descriptor3, descriptor4, descriptor5, descriptor6}; } diff --git a/LibCarla/source/carla/ros2/publishers/CarlaSemanticLidarPublisher.h b/LibCarla/source/carla/ros2/publishers/CarlaSemanticLidarPublisher.h index a76e57c01fb..57252de7db7 100644 --- a/LibCarla/source/carla/ros2/publishers/CarlaSemanticLidarPublisher.h +++ b/LibCarla/source/carla/ros2/publishers/CarlaSemanticLidarPublisher.h @@ -19,7 +19,7 @@ namespace ros2 { private: size_t GetPointSize() override; - std::vector GetFields() override; + std::vector GetFields() override; std::vector ComputePointCloud(uint32_t height, uint32_t width, uint8_t *data) override; }; diff --git a/LibCarla/source/carla/ros2/publishers/CarlaTransformPublisher.cpp b/LibCarla/source/carla/ros2/publishers/CarlaTransformPublisher.cpp index 10e3c985d55..3358f50bd08 100644 --- a/LibCarla/source/carla/ros2/publishers/CarlaTransformPublisher.cpp +++ b/LibCarla/source/carla/ros2/publishers/CarlaTransformPublisher.cpp @@ -9,7 +9,7 @@ namespace ros2 { constexpr double EPSILON = 1e-4; -geometry_msgs::msg::Transform CarlaTransformPublisher::ComputeTransform(std::string frame_id, geom::Transform transform) { +msg::Transform CarlaTransformPublisher::ComputeTransform(std::string frame_id, geom::Transform transform) { // Avoid recomputing the transform if it hasn't changed. // This is common for static sensors that are typically attached to other actors. @@ -49,35 +49,34 @@ geometry_msgs::msg::Transform CarlaTransformPublisher::ComputeTransform(std::str const float cy = cosf(ry * 0.5f); const float sy = sinf(ry * 0.5f); - geometry_msgs::msg::Transform tf; + msg::Transform tf; - tf.translation().x(tx); - tf.translation().y(ty); - tf.translation().z(tz); + tf.translation.x = tx; + tf.translation.y = ty; + tf.translation.z = tz; - tf.rotation().w(cr * cp * cy + sr * sp * sy); - tf.rotation().x(sr * cp * cy - cr * sp * sy); - tf.rotation().y(cr * sp * cy + sr * cp * sy); - tf.rotation().z(cr * cp * sy - sr * sp * cy); + tf.rotation.w = cr * cp * cy + sr * sp * sy; + tf.rotation.x = sr * cp * cy - cr * sp * sy; + tf.rotation.y = cr * sp * cy + sr * cp * sy; + tf.rotation.z = cr * cp * sy - sr * sp * cy; return tf; } bool CarlaTransformPublisher::Write(int32_t seconds, uint32_t nanoseconds, std::string frame_id, std::string child_frame_id, geom::Transform transform) { + msg::TransformStamped ts; - geometry_msgs::msg::TransformStamped ts; - - ts.header().stamp().sec(seconds); - ts.header().stamp().nanosec(nanoseconds); - ts.header().frame_id(frame_id); + ts.header.stamp.sec = seconds; + ts.header.stamp.nanosec = nanoseconds; + ts.header.frame_id = frame_id; auto tf = ComputeTransform(child_frame_id, transform); - ts.transform(tf); + ts.transform = tf; - ts.child_frame_id(child_frame_id); + ts.child_frame_id = child_frame_id; - _impl->GetMessage()->transforms({ts}); + _impl->GetMessage()->transforms = {ts}; // Update last transform information _last_transforms.insert({child_frame_id, {transform, tf}}); diff --git a/LibCarla/source/carla/ros2/publishers/CarlaTransformPublisher.h b/LibCarla/source/carla/ros2/publishers/CarlaTransformPublisher.h index 82bbe9125c8..d3f3749d337 100644 --- a/LibCarla/source/carla/ros2/publishers/CarlaTransformPublisher.h +++ b/LibCarla/source/carla/ros2/publishers/CarlaTransformPublisher.h @@ -12,8 +12,8 @@ #include "carla/ros2/publishers/BasePublisher.h" #include "carla/ros2/publishers/PublisherImpl.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/Transform.h" namespace carla { namespace ros2 { @@ -21,14 +21,15 @@ namespace ros2 { class CarlaTransformPublisher : public BasePublisher { public: struct TransformMsgTraits { - using msg_type = tf2_msgs::msg::TFMessage; - using msg_pubsub_type = tf2_msgs::msg::TFMessagePubSubType; + using msg_type = msg::TFMessage; }; CarlaTransformPublisher() : BasePublisher("rt/tf"), _impl(std::make_shared>()) { - _impl->Init(GetBaseTopicName()); + if (!_impl->Init(GetBaseTopicName())) { + log_warning("CarlaTransformPublisher: Init failed for topic: ", GetBaseTopicName()); + } } bool Publish() { @@ -38,12 +39,12 @@ namespace ros2 { bool Write(int32_t seconds, uint32_t nanoseconds, std::string frame_id, std::string child_frame_id, geom::Transform transform); private: - geometry_msgs::msg::Transform ComputeTransform(std::string frame_id, geom::Transform current_transform); + msg::Transform ComputeTransform(std::string frame_id, geom::Transform current_transform); private: std::shared_ptr> _impl; - std::unordered_map> _last_transforms; + std::unordered_map> _last_transforms; }; } // namespace ros2 diff --git a/LibCarla/source/carla/ros2/subscribers/AckermannControlSubscriber.cpp b/LibCarla/source/carla/ros2/subscribers/AckermannControlSubscriber.cpp index 5a670c14a60..f6543071525 100644 --- a/LibCarla/source/carla/ros2/subscribers/AckermannControlSubscriber.cpp +++ b/LibCarla/source/carla/ros2/subscribers/AckermannControlSubscriber.cpp @@ -1,3 +1,7 @@ +// 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 . + #include "AckermannControlSubscriber.h" #include "carla/ros2/ROS2CallbackData.h" @@ -9,11 +13,11 @@ namespace ros2 { auto message = _impl->GetMessage(); AckermannControl control; - control.steer = message.drive().steering_angle(); - control.steer_speed = message.drive().steering_angle_velocity(); - control.speed = message.drive().speed(); - control.acceleration = message.drive().acceleration(); - control.jerk = message.drive().jerk(); + control.steer = message.drive.steering_angle; + control.steer_speed = message.drive.steering_angle_velocity; + control.speed = message.drive.speed; + control.acceleration = message.drive.acceleration; + control.jerk = message.drive.jerk; return control; } diff --git a/LibCarla/source/carla/ros2/subscribers/AckermannControlSubscriber.h b/LibCarla/source/carla/ros2/subscribers/AckermannControlSubscriber.h index 36749b4c098..6a306d28fd0 100644 --- a/LibCarla/source/carla/ros2/subscribers/AckermannControlSubscriber.h +++ b/LibCarla/source/carla/ros2/subscribers/AckermannControlSubscriber.h @@ -9,8 +9,7 @@ #include "BaseSubscriber.h" #include "SubscriberImpl.h" -#include "carla/ros2/types/AckermannDriveStamped.h" -#include "carla/ros2/types/AckermannDriveStampedPubSubTypes.h" +#include "carla/ros2/types/msg/AckermannDriveStamped.h" #include "carla/ros2/ROS2CallbackData.h" @@ -20,15 +19,17 @@ namespace ros2 { class AckermannControlSubscriber : public BaseSubscriber { public: struct AckermannMsgTraits { - using msg_type = ackermann_msgs::msg::AckermannDriveStamped; - using msg_pubsub_type = ackermann_msgs::msg::AckermannDriveStampedPubSubType; + using msg_type = msg::AckermannDriveStamped; }; AckermannControlSubscriber(void* vehicle, std::string base_topic_name, std::string frame_id) : BaseSubscriber(vehicle, base_topic_name, frame_id), _impl(std::make_shared>()) { - _impl->Init(this->GetBaseTopicName() + "/ackermann_control_cmd"); + if (!_impl->Init(this->GetBaseTopicName() + "/ackermann_control_cmd")) { + log_warning("AckermannControlSubscriber: Init failed for topic: ", + this->GetBaseTopicName() + "/ackermann_control_cmd"); + } } ROS2CallbackData GetMessage(); diff --git a/LibCarla/source/carla/ros2/subscribers/CarlaEgoVehicleControlSubscriber.cpp b/LibCarla/source/carla/ros2/subscribers/CarlaEgoVehicleControlSubscriber.cpp index d3d3e7b646b..e63cd675c46 100644 --- a/LibCarla/source/carla/ros2/subscribers/CarlaEgoVehicleControlSubscriber.cpp +++ b/LibCarla/source/carla/ros2/subscribers/CarlaEgoVehicleControlSubscriber.cpp @@ -1,3 +1,7 @@ +// 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 . + #include "CarlaEgoVehicleControlSubscriber.h" #include "carla/ros2/ROS2CallbackData.h" @@ -9,13 +13,13 @@ namespace ros2 { 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/CarlaEgoVehicleControlSubscriber.h b/LibCarla/source/carla/ros2/subscribers/CarlaEgoVehicleControlSubscriber.h index 39f108fb30d..573db35e700 100644 --- a/LibCarla/source/carla/ros2/subscribers/CarlaEgoVehicleControlSubscriber.h +++ b/LibCarla/source/carla/ros2/subscribers/CarlaEgoVehicleControlSubscriber.h @@ -9,8 +9,7 @@ #include "BaseSubscriber.h" #include "SubscriberImpl.h" -#include "carla/ros2/types/CarlaEgoVehicleControl.h" -#include "carla/ros2/types/CarlaEgoVehicleControlPubSubTypes.h" +#include "carla/ros2/types/msg/CarlaEgoVehicleControl.h" #include "carla/ros2/ROS2CallbackData.h" @@ -20,15 +19,17 @@ namespace ros2 { class CarlaEgoVehicleControlSubscriber : public BaseSubscriber { public: struct ControlMsgTraits { - using msg_type = carla_msgs::msg::CarlaEgoVehicleControl; - using msg_pubsub_type = carla_msgs::msg::CarlaEgoVehicleControlPubSubType; + using msg_type = msg::CarlaEgoVehicleControl; }; CarlaEgoVehicleControlSubscriber(void* vehicle, std::string base_topic_name, std::string frame_id) : BaseSubscriber(vehicle, base_topic_name, frame_id), _impl(std::make_shared>()) { - _impl->Init(this->GetBaseTopicName() + "/vehicle_control_cmd"); + if (!_impl->Init(this->GetBaseTopicName() + "/vehicle_control_cmd")) { + log_warning("CarlaEgoVehicleControlSubscriber: Init failed for topic: ", + this->GetBaseTopicName() + "/vehicle_control_cmd"); + } } ROS2CallbackData GetMessage(); From 2a4a8db3def0d426be56f7c303cee1ebc931a4b1 Mon Sep 17 00:00:00 2001 From: Jesus Armando Anaya Date: Mon, 30 Mar 2026 02:12:49 -0700 Subject: [PATCH 4/7] feat(LibCarla/ros2): add CycloneDDS to middleware enum, factory, and tests Register CycloneDDS as a recognized middleware option in DDSMiddleware enum, DDSMiddlewareToString, DDSMiddlewareFromString, and GetAvailableMiddlewareString. Add CycloneDDS case to DDSMiddlewareFactory for IsMiddlewareAvailable, CreatePublisher, and CreateSubscriber, all guarded by CARLA_ROS2_DDS_CYCLONEDDS. Add stub CycloneDDSPublisherMiddleware and CycloneDDSSubscriberMiddleware that compile and satisfy the factory interface but return failure on all operations until the real implementation lands. Add 7 new unit tests for CycloneDDS enum parsing, availability checks, and factory routing when the middleware is not compiled in. --- .../source/carla/ros2/dds/DDSMiddleware.h | 14 +++- .../carla/ros2/dds/DDSMiddlewareFactory.h | 27 +++++++ .../CycloneDDSPublisherMiddleware.h | 45 ++++++++++++ .../CycloneDDSSubscriberMiddleware.h | 44 ++++++++++++ .../test/server/test_dds_middleware.cpp | 70 ++++++++++++++++--- 5 files changed, 189 insertions(+), 11 deletions(-) create mode 100644 LibCarla/source/carla/ros2/dds/cyclonedds/CycloneDDSPublisherMiddleware.h create mode 100644 LibCarla/source/carla/ros2/dds/cyclonedds/CycloneDDSSubscriberMiddleware.h diff --git a/LibCarla/source/carla/ros2/dds/DDSMiddleware.h b/LibCarla/source/carla/ros2/dds/DDSMiddleware.h index 20f8804c628..83811d7ad87 100644 --- a/LibCarla/source/carla/ros2/dds/DDSMiddleware.h +++ b/LibCarla/source/carla/ros2/dds/DDSMiddleware.h @@ -13,7 +13,8 @@ namespace ros2 { /// Passed to ROS2::Enable() to select the middleware at startup. /// Once set, the middleware cannot be changed without restarting. enum class DDSMiddleware { - FastDDS + FastDDS, + CycloneDDS }; /// Convert a DDSMiddleware enum value to a readable string. @@ -21,6 +22,8 @@ inline const char* DDSMiddlewareToString(DDSMiddleware middleware) { switch (middleware) { case DDSMiddleware::FastDDS: return "FastDDS"; + case DDSMiddleware::CycloneDDS: + return "CycloneDDS"; } return "Unknown"; } @@ -37,6 +40,9 @@ inline DDSMiddlewareParseResult DDSMiddlewareFromString(const std::string& name) if (name == "fastdds") { return {true, DDSMiddleware::FastDDS}; } + if (name == "cyclonedds") { + return {true, DDSMiddleware::CycloneDDS}; + } return {false, DDSMiddleware::FastDDS}; } @@ -45,6 +51,12 @@ inline std::string GetAvailableMiddlewareString() { std::string result; #if defined(CARLA_ROS2_DDS_FASTDDS) result += "FastDDS"; +#endif +#if defined(CARLA_ROS2_DDS_CYCLONEDDS) + if (!result.empty()) { + result += ", "; + } + result += "CycloneDDS"; #endif if (result.empty()) { result = "none"; diff --git a/LibCarla/source/carla/ros2/dds/DDSMiddlewareFactory.h b/LibCarla/source/carla/ros2/dds/DDSMiddlewareFactory.h index 51cc8f81143..36adc6e6570 100644 --- a/LibCarla/source/carla/ros2/dds/DDSMiddlewareFactory.h +++ b/LibCarla/source/carla/ros2/dds/DDSMiddlewareFactory.h @@ -17,6 +17,11 @@ # include "carla/ros2/dds/fastdds/FastDDSSubscriberMiddleware.h" #endif +#if defined(CARLA_ROS2_DDS_CYCLONEDDS) && !defined(CARLA_ROS2_DDS_TESTING) +# include "carla/ros2/dds/cyclonedds/CycloneDDSPublisherMiddleware.h" +# include "carla/ros2/dds/cyclonedds/CycloneDDSSubscriberMiddleware.h" +#endif + namespace carla { namespace ros2 { @@ -44,6 +49,12 @@ class DDSMiddlewareFactory { return true; #else return false; +#endif + case DDSMiddleware::CycloneDDS: +#if defined(CARLA_ROS2_DDS_CYCLONEDDS) + return true; +#else + return false; #endif } return false; @@ -83,6 +94,14 @@ class DDSMiddlewareFactory { #else log_error("DDSMiddlewareFactory: FastDDS not compiled in"); return nullptr; +#endif + case DDSMiddleware::CycloneDDS: +#if defined(CARLA_ROS2_DDS_CYCLONEDDS) && !defined(CARLA_ROS2_DDS_TESTING) + return std::unique_ptr( + new CycloneDDSPublisherMiddleware()); +#else + log_error("DDSMiddlewareFactory: CycloneDDS not compiled in"); + return nullptr; #endif } return nullptr; @@ -101,6 +120,14 @@ class DDSMiddlewareFactory { #else log_error("DDSMiddlewareFactory: FastDDS not compiled in"); return nullptr; +#endif + case DDSMiddleware::CycloneDDS: +#if defined(CARLA_ROS2_DDS_CYCLONEDDS) && !defined(CARLA_ROS2_DDS_TESTING) + return std::unique_ptr( + new CycloneDDSSubscriberMiddleware()); +#else + log_error("DDSMiddlewareFactory: CycloneDDS not compiled in"); + return nullptr; #endif } return nullptr; diff --git a/LibCarla/source/carla/ros2/dds/cyclonedds/CycloneDDSPublisherMiddleware.h b/LibCarla/source/carla/ros2/dds/cyclonedds/CycloneDDSPublisherMiddleware.h new file mode 100644 index 00000000000..383b937056d --- /dev/null +++ b/LibCarla/source/carla/ros2/dds/cyclonedds/CycloneDDSPublisherMiddleware.h @@ -0,0 +1,45 @@ +// 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/dds/IDDSPublisherMiddleware.h" +#include "carla/Logging.h" + +namespace carla { +namespace ros2 { + +/// CycloneDDS implementation of IDDSPublisherMiddleware (stub). +/// +/// This is a placeholder that compiles and satisfies the factory interface +/// but does not contain a real CycloneDDS implementation. All operations +/// log an error and return failure. The real implementation will replace +/// this file once CycloneDDS types and the C API are available. +/// +/// Parameterized on a traits type T that provides: +/// T::msg_type — middleware-neutral POD message struct +template +class CycloneDDSPublisherMiddleware : public IDDSPublisherMiddleware { + public: + bool Init(const std::string& topic_name) override { + log_error("CycloneDDSPublisherMiddleware: stub — not yet implemented " + "(topic '", topic_name, "')"); + return false; + } + + bool Publish(void* /*message_data*/) override { + return false; + } + + bool IsAlive() const override { + return false; + } + + std::string GetTopicName() const override { + return {}; + } +}; + +} // namespace ros2 +} // namespace carla diff --git a/LibCarla/source/carla/ros2/dds/cyclonedds/CycloneDDSSubscriberMiddleware.h b/LibCarla/source/carla/ros2/dds/cyclonedds/CycloneDDSSubscriberMiddleware.h new file mode 100644 index 00000000000..174ff99b8b5 --- /dev/null +++ b/LibCarla/source/carla/ros2/dds/cyclonedds/CycloneDDSSubscriberMiddleware.h @@ -0,0 +1,44 @@ +// 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/dds/IDDSSubscriberMiddleware.h" +#include "carla/Logging.h" + +namespace carla { +namespace ros2 { + +/// CycloneDDS implementation of IDDSSubscriberMiddleware (stub). +/// +/// This is a placeholder that compiles and satisfies the factory interface +/// but does not contain a real CycloneDDS implementation. All operations +/// log an error and return failure. The real implementation will replace +/// this file once CycloneDDS types and the C API are available. +/// +/// Parameterized on a traits type S that provides: +/// S::msg_type — middleware-neutral POD message struct +template +class CycloneDDSSubscriberMiddleware : public IDDSSubscriberMiddleware { + public: + bool Init( + const std::string& topic_name, + void* /*message_ptr*/, + bool* /*new_message_flag*/) override { + log_error("CycloneDDSSubscriberMiddleware: stub — not yet implemented " + "(topic '", topic_name, "')"); + return false; + } + + bool IsAlive() const override { + return false; + } + + std::string GetTopicName() const override { + return {}; + } +}; + +} // namespace ros2 +} // namespace carla diff --git a/LibCarla/source/test/server/test_dds_middleware.cpp b/LibCarla/source/test/server/test_dds_middleware.cpp index a4563b94c81..5ec7116292c 100644 --- a/LibCarla/source/test/server/test_dds_middleware.cpp +++ b/LibCarla/source/test/server/test_dds_middleware.cpp @@ -107,19 +107,26 @@ class DDSMiddlewareFactoryFixture : public ::testing::Test { // ========================================================================== TEST(dds_middleware_enum, values_exist) { - DDSMiddleware mw = DDSMiddleware::FastDDS; - EXPECT_EQ(static_cast(mw), 0); + DDSMiddleware mw_fast = DDSMiddleware::FastDDS; + EXPECT_EQ(static_cast(mw_fast), 0); + DDSMiddleware mw_cyclone = DDSMiddleware::CycloneDDS; + EXPECT_EQ(static_cast(mw_cyclone), 1); } TEST(dds_middleware_enum, switch_covers_all) { - DDSMiddleware mw = DDSMiddleware::FastDDS; - bool covered = false; - switch (mw) { - case DDSMiddleware::FastDDS: - covered = true; - break; + DDSMiddleware values[] = {DDSMiddleware::FastDDS, DDSMiddleware::CycloneDDS}; + for (auto mw : values) { + bool covered = false; + switch (mw) { + case DDSMiddleware::FastDDS: + covered = true; + break; + case DDSMiddleware::CycloneDDS: + covered = true; + break; + } + EXPECT_TRUE(covered); } - EXPECT_TRUE(covered); } // ========================================================================== @@ -140,6 +147,10 @@ TEST(dds_middleware_to_string, result_is_not_empty) { EXPECT_STRNE(result, ""); } +TEST(dds_middleware_to_string, cyclonedds_returns_correct_string) { + EXPECT_STREQ(DDSMiddlewareToString(DDSMiddleware::CycloneDDS), "CycloneDDS"); +} + // ========================================================================== // Group 3: dds_middleware_from_string (5 tests) // ========================================================================== @@ -151,10 +162,16 @@ TEST(dds_middleware_from_string, fastdds_lowercase_valid) { } TEST(dds_middleware_from_string, unknown_string_invalid) { - auto result = DDSMiddlewareFromString("cyclonedds"); + auto result = DDSMiddlewareFromString("unknowndds"); EXPECT_FALSE(result.valid); } +TEST(dds_middleware_from_string, cyclonedds_lowercase_valid) { + auto result = DDSMiddlewareFromString("cyclonedds"); + EXPECT_TRUE(result.valid); + EXPECT_EQ(result.middleware, DDSMiddleware::CycloneDDS); +} + TEST(dds_middleware_from_string, empty_string_invalid) { auto result = DDSMiddlewareFromString(""); EXPECT_FALSE(result.valid); @@ -184,6 +201,11 @@ TEST(dds_middleware_available, available_string_contains_fastdds) { EXPECT_NE(available.find("FastDDS"), std::string::npos); } +TEST(dds_middleware_available, cyclonedds_not_available_without_macro) { + EXPECT_FALSE( + DDSMiddlewareFactory::IsMiddlewareAvailable(DDSMiddleware::CycloneDDS)); +} + // ========================================================================== // Group 5: dds_middleware_type_name (4 tests) // ========================================================================== @@ -236,6 +258,34 @@ TEST_F(DDSMiddlewareFactoryFixture, factory_available_string) { EXPECT_NE(available.find("FastDDS"), std::string::npos); } +TEST_F(DDSMiddlewareFactoryFixture, set_and_get_cyclonedds) { + DDSMiddlewareFactory::SetMiddleware(DDSMiddleware::CycloneDDS); + EXPECT_EQ(DDSMiddlewareFactory::GetMiddleware(), DDSMiddleware::CycloneDDS); +} + +TEST_F(DDSMiddlewareFactoryFixture, resolve_unavailable_cyclonedds) { + auto resolution = + DDSMiddlewareFactory::ResolveMiddleware(DDSMiddleware::CycloneDDS); + EXPECT_FALSE(resolution.success); + EXPECT_EQ(resolution.middleware, DDSMiddleware::CycloneDDS); +} + +TEST_F(DDSMiddlewareFactoryFixture, create_publisher_cyclonedds_unavailable) { + DDSMiddlewareFactory::SetMiddleware(DDSMiddleware::CycloneDDS); + ::testing::internal::CaptureStderr(); + auto pub = DDSMiddlewareFactory::CreatePublisher(); + ::testing::internal::GetCapturedStderr(); + EXPECT_EQ(pub, nullptr); +} + +TEST_F(DDSMiddlewareFactoryFixture, create_subscriber_cyclonedds_unavailable) { + DDSMiddlewareFactory::SetMiddleware(DDSMiddleware::CycloneDDS); + ::testing::internal::CaptureStderr(); + auto sub = DDSMiddlewareFactory::CreateSubscriber(); + ::testing::internal::GetCapturedStderr(); + EXPECT_EQ(sub, nullptr); +} + // ========================================================================== // Group 7: dds_publisher_interface (5 tests) // ========================================================================== From 5a9dd3b4b9607e71017d9d59de631e21c58c0453 Mon Sep 17 00:00:00 2001 From: Jesus Armando Anaya Date: Sat, 4 Apr 2026 18:48:44 -0700 Subject: [PATCH 5/7] feat(LibCarla/ros2): add CdrSerialization, CdrTopicInfo, and CDR round-trip tests MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Add CdrSerialization.h with serialize_to_cdr() / deserialize_from_cdr() helpers for all 31 msg::* POD types using Fast-CDR (XCDR1 LE with DDS encapsulation header). The resulting buffers are wire-compatible with all ROS2 distros and can be handed directly to FastDDS write_serialized_payload() or CycloneDDS dds_writecdr(), removing the need for per-vendor generated type files. Add CdrTopicInfo.h with type_name() and max_serialized_size() template specializations for all 31 types, providing the metadata the DDS middleware layer needs to register topics and pre-allocate payload buffers. Add 14 CDR round-trip tests across two new groups (cdr_topic_info, cdr_serialization) covering fixed-size, string, array, and vector-of-struct types. Update cmake/test/CMakeLists.txt to link libfastcdr into the server test binary and enable exceptions, which Fast-CDR templates require. Server test count: 97 → 111. Blocked on: feature/ros2-cyclonedds-enum-factory (#9620) --- LibCarla/cmake/test/CMakeLists.txt | 9 + .../carla/ros2/types/CdrSerialization.h | 659 ++++++++++++++++++ .../source/carla/ros2/types/CdrTopicInfo.h | 284 ++++++++ .../test/server/test_dds_middleware.cpp | 300 ++++++++ 4 files changed, 1252 insertions(+) create mode 100644 LibCarla/source/carla/ros2/types/CdrSerialization.h create mode 100644 LibCarla/source/carla/ros2/types/CdrTopicInfo.h diff --git a/LibCarla/cmake/test/CMakeLists.txt b/LibCarla/cmake/test/CMakeLists.txt index 432f509e726..864fbbd87f5 100644 --- a/LibCarla/cmake/test/CMakeLists.txt +++ b/LibCarla/cmake/test/CMakeLists.txt @@ -54,6 +54,15 @@ foreach(target ${build_targets}) target_include_directories(${target} PRIVATE "${libcarla_source_path}/test") + # Server tests exercise CDR serialization (CdrSerialization.h) which + # requires Fast-CDR headers and the libfastcdr runtime. Enable exceptions + # for this target because Fast-CDR templates use try/catch internally. + if (CMAKE_BUILD_TYPE STREQUAL "Server") + target_include_directories(${target} SYSTEM PRIVATE "${FASTDDS_INCLUDE_PATH}") + target_compile_options(${target} PRIVATE -fexceptions) + target_link_libraries(${target} "${FASTDDS_LIB_PATH}/libfastcdr.a") + endif() + if (WIN32) target_link_libraries(${target} "gtest_main.lib") target_link_libraries(${target} "gtest.lib") diff --git a/LibCarla/source/carla/ros2/types/CdrSerialization.h b/LibCarla/source/carla/ros2/types/CdrSerialization.h new file mode 100644 index 00000000000..2c072a2554f --- /dev/null +++ b/LibCarla/source/carla/ros2/types/CdrSerialization.h @@ -0,0 +1,659 @@ +// 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/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. +// ========================================================================== + +// -------------------------------------------------------------------------- +// 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 << 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; + int32_t fields_size{0}; + cdr >> fields_size; + 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 << 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) { + int32_t transforms_size{0}; + cdr >> transforms_size; + 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 (XCDR1 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(). +template +std::vector serialize_to_cdr(const T& msg) { + eprosima::fastcdr::FastBuffer fb; + eprosima::fastcdr::Cdr cdr{ + fb, + eprosima::fastcdr::Cdr::DEFAULT_ENDIAN, + eprosima::fastcdr::Cdr::DDS_CDR}; + cdr.serialize_encapsulation(); + serialize_cdr(cdr, msg); + const char* buf{fb.getBuffer()}; + const size_t len{cdr.getSerializedDataLength()}; + return std::vector{ + reinterpret_cast(buf), + reinterpret_cast(buf) + len}; +} + +/// 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. The buffer must include the 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::DEFAULT_ENDIAN, + eprosima::fastcdr::Cdr::DDS_CDR}; + cdr.read_encapsulation(); + deserialize_cdr(cdr, msg); + 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..e802e7215a7 --- /dev/null +++ b/LibCarla/source/carla/ros2/types/CdrTopicInfo.h @@ -0,0 +1,284 @@ +// 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 DDS middleware layer. +/// +/// type_name() — ROS2-compatible DDS type name string used when +/// registering the type with a DomainParticipant. +/// Follows the "pkg::msg::dds_::TypeName_" pattern. +/// +/// max_serialized_size() — Conservative upper bound on the CDR payload size +/// in bytes, excluding the 4-byte DDS encapsulation +/// header. Used to pre-allocate topic buffers. +/// For types with variable-length fields (strings, +/// vectors) this is a practical maximum, not +/// an absolute one. +/// +/// 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 size_t max_serialized_size() { return 20u; } +}; + +template<> struct CdrTopicInfo { + static const char* type_name() { + return "ackermann_msgs::msg::dds_::AckermannDriveStamped_"; + } + static size_t max_serialized_size() { return 288u; } +}; + +template<> struct CdrTopicInfo { + static const char* type_name() { + return "sensor_msgs::msg::dds_::CameraInfo_"; + } + static size_t max_serialized_size() { return 3793u; } +}; + +template<> struct CdrTopicInfo { + static const char* type_name() { + return "carla_msgs::msg::dds_::CarlaCollisionEvent_"; + } + static size_t max_serialized_size() { return 296u; } +}; + +template<> struct CdrTopicInfo { + static const char* type_name() { + return "carla_msgs::msg::dds_::CarlaEgoVehicleControl_"; + } + static size_t max_serialized_size() { return 289u; } +}; + +template<> struct CdrTopicInfo { + static const char* type_name() { + return "carla_msgs::msg::dds_::LaneInvasionEvent_"; + } + static size_t max_serialized_size() { return 672u; } +}; + +template<> struct CdrTopicInfo { + static const char* type_name() { + return "rosgraph_msgs::msg::dds_::Clock_"; + } + // 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 size_t max_serialized_size() { return 4u; } +}; + +template<> struct CdrTopicInfo { + static const char* type_name() { + return "std_msgs::msg::dds_::Header_"; + } + static size_t max_serialized_size() { return 268u; } +}; + +template<> struct CdrTopicInfo { + static const char* type_name() { + return "sensor_msgs::msg::dds_::Image_"; + } + static size_t max_serialized_size() { return 648u; } +}; + +template<> struct CdrTopicInfo { + static const char* type_name() { + return "sensor_msgs::msg::dds_::Imu_"; + } + static size_t max_serialized_size() { return 568u; } +}; + +template<> struct CdrTopicInfo { + static const char* type_name() { + return "sensor_msgs::msg::dds_::NavSatFix_"; + } + static size_t max_serialized_size() { return 369u; } +}; + +template<> struct CdrTopicInfo { + static const char* type_name() { + return "sensor_msgs::msg::dds_::NavSatStatus_"; + } + static size_t max_serialized_size() { return 4u; } +}; + +template<> struct CdrTopicInfo { + static const char* type_name() { + return "nav_msgs::msg::dds_::Odometry_"; + } + static size_t max_serialized_size() { return 1208u; } +}; + +template<> struct CdrTopicInfo { + static const char* type_name() { + return "geometry_msgs::msg::dds_::Point_"; + } + static size_t max_serialized_size() { return 24u; } +}; + +template<> struct CdrTopicInfo { + static const char* type_name() { + return "geometry_msgs::msg::dds_::Point32_"; + } + static size_t max_serialized_size() { return 12u; } +}; + +template<> struct CdrTopicInfo { + static const char* type_name() { + return "sensor_msgs::msg::dds_::PointCloud2_"; + } + static size_t max_serialized_size() { return 27597u; } +}; + +template<> struct CdrTopicInfo { + static const char* type_name() { + return "sensor_msgs::msg::dds_::PointField_"; + } + static size_t max_serialized_size() { return 272u; } +}; + +template<> struct CdrTopicInfo { + static const char* type_name() { + return "geometry_msgs::msg::dds_::Pose_"; + } + static size_t max_serialized_size() { return 56u; } +}; + +template<> struct CdrTopicInfo { + static const char* type_name() { + return "geometry_msgs::msg::dds_::PoseWithCovariance_"; + } + static size_t max_serialized_size() { return 344u; } +}; + +template<> struct CdrTopicInfo { + static const char* type_name() { + return "geometry_msgs::msg::dds_::Quaternion_"; + } + static size_t max_serialized_size() { return 32u; } +}; + +template<> struct CdrTopicInfo { + static const char* type_name() { + return "sensor_msgs::msg::dds_::RegionOfInterest_"; + } + static size_t max_serialized_size() { return 17u; } +}; + +template<> struct CdrTopicInfo { + static const char* type_name() { + return "std_msgs::msg::dds_::String_"; + } + static size_t max_serialized_size() { return 260u; } +}; + +template<> struct CdrTopicInfo { + static const char* type_name() { + return "tf2_msgs::msg::dds_::TF2Error_"; + } + static size_t max_serialized_size() { return 264u; } +}; + +template<> struct CdrTopicInfo { + static const char* type_name() { + return "tf2_msgs::msg::dds_::TFMessage_"; + } + static size_t max_serialized_size() { return 58408u; } +}; + +template<> struct CdrTopicInfo { + static const char* type_name() { + return "builtin_interfaces::msg::dds_::Time_"; + } + static size_t max_serialized_size() { return 8u; } +}; + +template<> struct CdrTopicInfo { + static const char* type_name() { + return "geometry_msgs::msg::dds_::Transform_"; + } + static size_t max_serialized_size() { return 56u; } +}; + +template<> struct CdrTopicInfo { + static const char* type_name() { + return "geometry_msgs::msg::dds_::TransformStamped_"; + } + static size_t max_serialized_size() { return 584u; } +}; + +template<> struct CdrTopicInfo { + static const char* type_name() { + return "geometry_msgs::msg::dds_::Twist_"; + } + static size_t max_serialized_size() { return 48u; } +}; + +template<> struct CdrTopicInfo { + static const char* type_name() { + return "geometry_msgs::msg::dds_::TwistWithCovariance_"; + } + static size_t max_serialized_size() { return 336u; } +}; + +template<> struct CdrTopicInfo { + static const char* type_name() { + return "geometry_msgs::msg::dds_::Vector3_"; + } + static size_t max_serialized_size() { return 24u; } +}; + +} // namespace ros2 +} // namespace carla diff --git a/LibCarla/source/test/server/test_dds_middleware.cpp b/LibCarla/source/test/server/test_dds_middleware.cpp index 5ec7116292c..d91d05f81a0 100644 --- a/LibCarla/source/test/server/test_dds_middleware.cpp +++ b/LibCarla/source/test/server/test_dds_middleware.cpp @@ -15,9 +15,14 @@ #include #include #include +#include +#include +#include +#include #include #include +#include using namespace carla::ros2; @@ -538,3 +543,298 @@ TEST(subscriber_impl, init_failure_propagated) { std::unique_ptr(mock)); EXPECT_FALSE(sub.Init("rt/test_topic")); } + +// ========================================================================== +// Group 9: 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 10: cdr_serialization (12 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); +} + +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()); +} From 0a9ca585ed136dd2675672deadceeabd3a649eef Mon Sep 17 00:00:00 2001 From: Jesus Armando Anaya Date: Sat, 4 Apr 2026 19:57:23 -0700 Subject: [PATCH 6/7] feat(LibCarla/ros2): add GenericCdrPubSubType, wire FastDDS middleware to CDR, add 6 tests Replace 30 fastddsgen-generated PubSubType classes (and FastDDSTypeMap.h) with a single GenericCdrPubSubType template that implements TopicDataType by delegating serialize()/deserialize() to CdrSerialization.h, and type_name()/ m_typeSize to CdrTopicInfo. Rewrite FastDDSPublisherMiddleware and FastDDSSubscriberMiddleware to use GenericCdrPubSubType instead of FastDDSTypeMap. Remove to_fastdds()/from_fastdds() conversion calls. Publishers now pass the POD msg pointer directly to DataWriter::write(); subscribers receive CDR payloads directly into _message_ptr. Add 6 generic_cdr_pubsubtype tests covering: type name contract, m_typeSize, serialize/deserialize round-trip via SerializedPayload_t (fixed-size and string types), createData/deleteData, and getKey(). Update cmake/test/CMakeLists.txt to also link libfastrtps.a and libfoonathan_memory (required by TopicDataType static initializers). Server test count: 111 to 117. --- LibCarla/cmake/test/CMakeLists.txt | 8 +- .../dds/fastdds/FastDDSPublisherMiddleware.h | 20 +- .../dds/fastdds/FastDDSSubscriberMiddleware.h | 19 +- .../carla/ros2/dds/fastdds/FastDDSTypeMap.h | 211 ------------------ .../ros2/dds/fastdds/GenericCdrPubSubType.h | 133 +++++++++++ .../test/server/test_dds_middleware.cpp | 99 ++++++++ 6 files changed, 251 insertions(+), 239 deletions(-) delete mode 100644 LibCarla/source/carla/ros2/dds/fastdds/FastDDSTypeMap.h create mode 100644 LibCarla/source/carla/ros2/dds/fastdds/GenericCdrPubSubType.h diff --git a/LibCarla/cmake/test/CMakeLists.txt b/LibCarla/cmake/test/CMakeLists.txt index 864fbbd87f5..a636b38f46a 100644 --- a/LibCarla/cmake/test/CMakeLists.txt +++ b/LibCarla/cmake/test/CMakeLists.txt @@ -54,13 +54,15 @@ foreach(target ${build_targets}) target_include_directories(${target} PRIVATE "${libcarla_source_path}/test") - # Server tests exercise CDR serialization (CdrSerialization.h) which - # requires Fast-CDR headers and the libfastcdr runtime. Enable exceptions - # for this target because Fast-CDR templates use try/catch internally. + # Server tests exercise CDR serialization (CdrSerialization.h) and + # GenericCdrPubSubType (inherits TopicDataType from fastrtps). + # Enable exceptions because Fast-CDR templates use try/catch internally. if (CMAKE_BUILD_TYPE STREQUAL "Server") target_include_directories(${target} SYSTEM PRIVATE "${FASTDDS_INCLUDE_PATH}") target_compile_options(${target} PRIVATE -fexceptions) + target_link_libraries(${target} "${FASTDDS_LIB_PATH}/libfastrtps.a") target_link_libraries(${target} "${FASTDDS_LIB_PATH}/libfastcdr.a") + target_link_libraries(${target} "${FASTDDS_LIB_PATH}/libfoonathan_memory-0.7.3.a") endif() if (WIN32) diff --git a/LibCarla/source/carla/ros2/dds/fastdds/FastDDSPublisherMiddleware.h b/LibCarla/source/carla/ros2/dds/fastdds/FastDDSPublisherMiddleware.h index 07ffec6bbaf..87e37f081e1 100644 --- a/LibCarla/source/carla/ros2/dds/fastdds/FastDDSPublisherMiddleware.h +++ b/LibCarla/source/carla/ros2/dds/fastdds/FastDDSPublisherMiddleware.h @@ -5,7 +5,7 @@ #pragma once #include "carla/ros2/dds/IDDSPublisherMiddleware.h" -#include "carla/ros2/dds/fastdds/FastDDSTypeMap.h" +#include "carla/ros2/dds/fastdds/GenericCdrPubSubType.h" #include "carla/Logging.h" #include @@ -30,17 +30,14 @@ using erc = eprosima::fastrtps::types::ReturnCode_t; /// FastDDS implementation of IDDSPublisherMiddleware. /// Parameterized on a traits type T that provides: -/// T::msg_type — the message type -/// Native FastDDS types are resolved via FastDDSTypeMap. +/// T::msg_type — the message type (a carla::ros2::msg::* POD struct) +/// Serialization is handled by GenericCdrPubSubType via CdrSerialization.h. template class FastDDSPublisherMiddleware : public IDDSPublisherMiddleware, public eprosima::fastdds::dds::DataWriterListener { public: using msg_type = typename T::msg_type; - using type_map = FastDDSTypeMap; - using fastdds_type = typename type_map::fastdds_type; - using fastdds_pubsub_type = typename type_map::fastdds_pubsub_type; void on_publication_matched( efd::DataWriter* writer, @@ -108,10 +105,8 @@ class FastDDSPublisherMiddleware } bool Publish(void* message_data) override { - auto* msg = static_cast(message_data); - to_fastdds(*msg, _fastdds_msg); eprosima::fastrtps::rtps::InstanceHandle_t instance_handle; - erc rcode = _datawriter->write(&_fastdds_msg, instance_handle); + erc rcode = _datawriter->write(message_data, instance_handle); if (rcode == erc::ReturnCodeValue::RETCODE_OK) { return true; } @@ -133,11 +128,10 @@ class FastDDSPublisherMiddleware efd::Publisher* _publisher { nullptr }; efd::Topic* _topic { nullptr }; efd::DataWriter* _datawriter { nullptr }; - efd::TypeSupport _type { new fastdds_pubsub_type() }; + efd::TypeSupport _type { new GenericCdrPubSubType() }; - fastdds_type _fastdds_msg; - std::string _topic_name; - bool _alive { false }; + std::string _topic_name; + bool _alive { false }; }; } // namespace ros2 diff --git a/LibCarla/source/carla/ros2/dds/fastdds/FastDDSSubscriberMiddleware.h b/LibCarla/source/carla/ros2/dds/fastdds/FastDDSSubscriberMiddleware.h index 03f50f757ba..c679f802580 100644 --- a/LibCarla/source/carla/ros2/dds/fastdds/FastDDSSubscriberMiddleware.h +++ b/LibCarla/source/carla/ros2/dds/fastdds/FastDDSSubscriberMiddleware.h @@ -5,7 +5,7 @@ #pragma once #include "carla/ros2/dds/IDDSSubscriberMiddleware.h" -#include "carla/ros2/dds/fastdds/FastDDSTypeMap.h" +#include "carla/ros2/dds/fastdds/GenericCdrPubSubType.h" #include "carla/Logging.h" #include @@ -31,17 +31,14 @@ using erc = eprosima::fastrtps::types::ReturnCode_t; /// FastDDS implementation of IDDSSubscriberMiddleware. /// Parameterized on traits type S that provides: -/// S::msg_type — the message type -/// Native FastDDS types are resolved via FastDDSTypeMap. +/// S::msg_type — the message type (a carla::ros2::msg::* POD struct) +/// Deserialization is handled by GenericCdrPubSubType via CdrSerialization.h. template class FastDDSSubscriberMiddleware : public IDDSSubscriberMiddleware, public eprosima::fastdds::dds::DataReaderListener { public: using msg_type = typename S::msg_type; - using type_map = FastDDSTypeMap; - using fastdds_type = typename type_map::fastdds_type; - using fastdds_pubsub_type = typename type_map::fastdds_pubsub_type; void on_subscription_matched( efd::DataReader* reader, @@ -51,9 +48,8 @@ class FastDDSSubscriberMiddleware void on_data_available(efd::DataReader* reader) override { efd::SampleInfo info; - erc rcode = reader->take_next_sample(&_fastdds_msg, &info); + erc rcode = reader->take_next_sample(_message_ptr, &info); if (rcode == erc::ReturnCodeValue::RETCODE_OK) { - from_fastdds(_fastdds_msg, *_message_ptr); *_new_message_ptr = true; } else { log_error("FastDDSSubscriberMiddleware::on_data_available (", @@ -137,11 +133,10 @@ class FastDDSSubscriberMiddleware efd::Subscriber* _subscriber { nullptr }; efd::Topic* _topic { nullptr }; efd::DataReader* _datareader { nullptr }; - efd::TypeSupport _type { new fastdds_pubsub_type() }; + efd::TypeSupport _type { new GenericCdrPubSubType() }; - fastdds_type _fastdds_msg; - msg_type* _message_ptr { nullptr }; - bool* _new_message_ptr { nullptr }; + msg_type* _message_ptr { nullptr }; + bool* _new_message_ptr { nullptr }; std::string _topic_name; bool _alive { false }; diff --git a/LibCarla/source/carla/ros2/dds/fastdds/FastDDSTypeMap.h b/LibCarla/source/carla/ros2/dds/fastdds/FastDDSTypeMap.h deleted file mode 100644 index 3770477df30..00000000000 --- a/LibCarla/source/carla/ros2/dds/fastdds/FastDDSTypeMap.h +++ /dev/null @@ -1,211 +0,0 @@ -// 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/FastDDSConversions.h" - -// PubSubType headers for type registration -#include "carla/ros2/types/TimePubSubTypes.h" -#include "carla/ros2/types/HeaderPubSubTypes.h" -#include "carla/ros2/types/Vector3PubSubTypes.h" -#include "carla/ros2/types/QuaternionPubSubTypes.h" -#include "carla/ros2/types/PointPubSubTypes.h" -#include "carla/ros2/types/Point32PubSubTypes.h" -#include "carla/ros2/types/PosePubSubTypes.h" -#include "carla/ros2/types/PoseWithCovariancePubSubTypes.h" -#include "carla/ros2/types/TwistPubSubTypes.h" -#include "carla/ros2/types/TwistWithCovariancePubSubTypes.h" -#include "carla/ros2/types/TransformPubSubTypes.h" -#include "carla/ros2/types/TransformStampedPubSubTypes.h" -#include "carla/ros2/types/OdometryPubSubTypes.h" -#include "carla/ros2/types/RegionOfInterestPubSubTypes.h" -#include "carla/ros2/types/PointFieldPubSubTypes.h" -#include "carla/ros2/types/NavSatStatusPubSubTypes.h" -#include "carla/ros2/types/NavSatFixPubSubTypes.h" -#include "carla/ros2/types/ClockPubSubTypes.h" -#include "carla/ros2/types/Float32PubSubTypes.h" -#include "carla/ros2/types/StringPubSubTypes.h" -#include "carla/ros2/types/ImuPubSubTypes.h" -#include "carla/ros2/types/ImagePubSubTypes.h" -#include "carla/ros2/types/CameraInfoPubSubTypes.h" -#include "carla/ros2/types/PointCloud2PubSubTypes.h" -#include "carla/ros2/types/TFMessagePubSubTypes.h" -#include "carla/ros2/types/TF2ErrorPubSubTypes.h" -#include "carla/ros2/types/AckermannDrivePubSubTypes.h" -#include "carla/ros2/types/AckermannDriveStampedPubSubTypes.h" -#include "carla/ros2/types/CarlaCollisionEventPubSubTypes.h" -#include "carla/ros2/types/CarlaEgoVehicleControlPubSubTypes.h" -#include "carla/ros2/types/CarlaLineInvasionPubSubTypes.h" - -namespace carla { -namespace ros2 { - -/// Maps a message type to its FastDDS native type and PubSubType. -/// Primary template is intentionally undefined — only specializations are valid. -template struct FastDDSTypeMap; - -// ============================================================ -// Specializations (POD msg types -> FastDDS types) -// These map middleware-neutral POD structs to FastDDS-generated types. -// Conversion functions are in types/FastDDSConversions.h. -// ============================================================ - -template<> struct FastDDSTypeMap { - using fastdds_type = builtin_interfaces::msg::Time; - using fastdds_pubsub_type = builtin_interfaces::msg::TimePubSubType; -}; - -template<> struct FastDDSTypeMap { - using fastdds_type = std_msgs::msg::Header; - using fastdds_pubsub_type = std_msgs::msg::HeaderPubSubType; -}; - -template<> struct FastDDSTypeMap { - using fastdds_type = geometry_msgs::msg::Vector3; - using fastdds_pubsub_type = geometry_msgs::msg::Vector3PubSubType; -}; - -template<> struct FastDDSTypeMap { - using fastdds_type = geometry_msgs::msg::Quaternion; - using fastdds_pubsub_type = geometry_msgs::msg::QuaternionPubSubType; -}; - -template<> struct FastDDSTypeMap { - using fastdds_type = geometry_msgs::msg::Point; - using fastdds_pubsub_type = geometry_msgs::msg::PointPubSubType; -}; - -template<> struct FastDDSTypeMap { - using fastdds_type = geometry_msgs::msg::Point32; - using fastdds_pubsub_type = geometry_msgs::msg::Point32PubSubType; -}; - -template<> struct FastDDSTypeMap { - using fastdds_type = geometry_msgs::msg::Pose; - using fastdds_pubsub_type = geometry_msgs::msg::PosePubSubType; -}; - -template<> struct FastDDSTypeMap { - using fastdds_type = geometry_msgs::msg::PoseWithCovariance; - using fastdds_pubsub_type = geometry_msgs::msg::PoseWithCovariancePubSubType; -}; - -template<> struct FastDDSTypeMap { - using fastdds_type = geometry_msgs::msg::Twist; - using fastdds_pubsub_type = geometry_msgs::msg::TwistPubSubType; -}; - -template<> struct FastDDSTypeMap { - using fastdds_type = geometry_msgs::msg::TwistWithCovariance; - using fastdds_pubsub_type = geometry_msgs::msg::TwistWithCovariancePubSubType; -}; - -template<> struct FastDDSTypeMap { - using fastdds_type = geometry_msgs::msg::Transform; - using fastdds_pubsub_type = geometry_msgs::msg::TransformPubSubType; -}; - -template<> struct FastDDSTypeMap { - using fastdds_type = geometry_msgs::msg::TransformStamped; - using fastdds_pubsub_type = geometry_msgs::msg::TransformStampedPubSubType; -}; - -template<> struct FastDDSTypeMap { - using fastdds_type = nav_msgs::msg::Odometry; - using fastdds_pubsub_type = nav_msgs::msg::OdometryPubSubType; -}; - -template<> struct FastDDSTypeMap { - using fastdds_type = sensor_msgs::msg::RegionOfInterest; - using fastdds_pubsub_type = sensor_msgs::msg::RegionOfInterestPubSubType; -}; - -template<> struct FastDDSTypeMap { - using fastdds_type = sensor_msgs::msg::PointField; - using fastdds_pubsub_type = sensor_msgs::msg::PointFieldPubSubType; -}; - -template<> struct FastDDSTypeMap { - using fastdds_type = sensor_msgs::msg::NavSatStatus; - using fastdds_pubsub_type = sensor_msgs::msg::NavSatStatusPubSubType; -}; - -template<> struct FastDDSTypeMap { - using fastdds_type = sensor_msgs::msg::NavSatFix; - using fastdds_pubsub_type = sensor_msgs::msg::NavSatFixPubSubType; -}; - -template<> struct FastDDSTypeMap { - using fastdds_type = rosgraph::msg::Clock; - using fastdds_pubsub_type = rosgraph::msg::ClockPubSubType; -}; - -template<> struct FastDDSTypeMap { - using fastdds_type = std_msgs::msg::Float32; - using fastdds_pubsub_type = std_msgs::msg::Float32PubSubType; -}; - -template<> struct FastDDSTypeMap { - using fastdds_type = std_msgs::msg::String; - using fastdds_pubsub_type = std_msgs::msg::StringPubSubType; -}; - -template<> struct FastDDSTypeMap { - using fastdds_type = sensor_msgs::msg::Imu; - using fastdds_pubsub_type = sensor_msgs::msg::ImuPubSubType; -}; - -template<> struct FastDDSTypeMap { - using fastdds_type = sensor_msgs::msg::Image; - using fastdds_pubsub_type = sensor_msgs::msg::ImagePubSubType; -}; - -template<> struct FastDDSTypeMap { - using fastdds_type = sensor_msgs::msg::CameraInfo; - using fastdds_pubsub_type = sensor_msgs::msg::CameraInfoPubSubType; -}; - -template<> struct FastDDSTypeMap { - using fastdds_type = sensor_msgs::msg::PointCloud2; - using fastdds_pubsub_type = sensor_msgs::msg::PointCloud2PubSubType; -}; - -template<> struct FastDDSTypeMap { - using fastdds_type = tf2_msgs::msg::TFMessage; - using fastdds_pubsub_type = tf2_msgs::msg::TFMessagePubSubType; -}; - -template<> struct FastDDSTypeMap { - using fastdds_type = tf2_msgs::msg::TF2Error; - using fastdds_pubsub_type = tf2_msgs::msg::TF2ErrorPubSubType; -}; - -template<> struct FastDDSTypeMap { - using fastdds_type = ackermann_msgs::msg::AckermannDrive; - using fastdds_pubsub_type = ackermann_msgs::msg::AckermannDrivePubSubType; -}; - -template<> struct FastDDSTypeMap { - using fastdds_type = ackermann_msgs::msg::AckermannDriveStamped; - using fastdds_pubsub_type = ackermann_msgs::msg::AckermannDriveStampedPubSubType; -}; - -template<> struct FastDDSTypeMap { - using fastdds_type = carla_msgs::msg::CarlaCollisionEvent; - using fastdds_pubsub_type = carla_msgs::msg::CarlaCollisionEventPubSubType; -}; - -template<> struct FastDDSTypeMap { - using fastdds_type = carla_msgs::msg::CarlaEgoVehicleControl; - using fastdds_pubsub_type = carla_msgs::msg::CarlaEgoVehicleControlPubSubType; -}; - -template<> struct FastDDSTypeMap { - using fastdds_type = carla_msgs::msg::LaneInvasionEvent; - using fastdds_pubsub_type = carla_msgs::msg::LaneInvasionEventPubSubType; -}; - -} // namespace ros2 -} // namespace carla diff --git a/LibCarla/source/carla/ros2/dds/fastdds/GenericCdrPubSubType.h b/LibCarla/source/carla/ros2/dds/fastdds/GenericCdrPubSubType.h new file mode 100644 index 00000000000..1bbc3382a2c --- /dev/null +++ b/LibCarla/source/carla/ros2/dds/fastdds/GenericCdrPubSubType.h @@ -0,0 +1,133 @@ +// 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/CdrSerialization.h" +#include "carla/ros2/types/CdrTopicInfo.h" + +#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 and FastDDSTypeMap<>. +/// 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 pre-allocated FastDDS payload buffer. + /// Called by FastDDS DataWriter::write() before sending on the wire. + bool serialize( + void* data, + SerializedPayload_t* payload) override { + const MsgType* msg = static_cast(data); + + eprosima::fastcdr::FastBuffer fastbuffer( + reinterpret_cast(payload->data), + static_cast(payload->max_size)); + eprosima::fastcdr::Cdr ser( + fastbuffer, + eprosima::fastcdr::Cdr::DEFAULT_ENDIAN, + eprosima::fastcdr::Cdr::DDS_CDR); + payload->encapsulation = (ser.endianness() == + eprosima::fastcdr::Cdr::BIG_ENDIANNESS) ? CDR_BE : CDR_LE; + + try { + ser.serialize_encapsulation(); + serialize_cdr(ser, *msg); + } catch (eprosima::fastcdr::exception::Exception& /*e*/) { + return false; + } + + payload->length = static_cast(ser.getSerializedDataLength()); + 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)); + eprosima::fastcdr::Cdr deser( + fastbuffer, + eprosima::fastcdr::Cdr::DEFAULT_ENDIAN, + 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 CDR-serialized size for the given instance. + /// FastDDS uses this to size the payload buffer before calling serialize(). + std::function getSerializedSizeProvider(void* /*data*/) override { + return []() -> uint32_t { + return static_cast( + CdrTopicInfo::max_serialized_size()) + 4u; + }; + } + + /// 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/LibCarla/source/test/server/test_dds_middleware.cpp b/LibCarla/source/test/server/test_dds_middleware.cpp index d91d05f81a0..53b5cdc06cb 100644 --- a/LibCarla/source/test/server/test_dds_middleware.cpp +++ b/LibCarla/source/test/server/test_dds_middleware.cpp @@ -15,6 +15,7 @@ #include #include #include +#include #include #include @@ -838,3 +839,101 @@ TEST(cdr_serialization, empty_tfmessage_round_trip) { EXPECT_TRUE(carla::ros2::deserialize_from_cdr(buf.data(), buf.size(), recovered)); EXPECT_TRUE(recovered.transforms.empty()); } + +// ========================================================================== +// Group 11: 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)); +} From 9502048d565067bf36ac4348666fc0788f239a40 Mon Sep 17 00:00:00 2001 From: Jesus Armando Anaya Date: Wed, 8 Apr 2026 05:04:05 -0700 Subject: [PATCH 7/7] fix(LibCarla/ros2): fix CDR spec violations, large-message publish, and add 9 robustness tests Fix four spec-compliance issues in CdrSerialization.h and GenericCdrPubSubType.h flagged during a deep review against the OMG DDSI-RTPS v2.5 spec and DDS-XTypes 1.3 clause 7.4.1.1, plus a critical runtime bug that prevented large-payload sensors (Image, PointCloud2) from publishing at all. --- Spec-compliance fixes --- A. Sequence length type (DDS-XTypes 7.4.1.1 violation) PointCloud2::fields and TFMessage::transforms used int32_t for CDR sequence lengths. DDS-XTypes 1.3 clause 7.4.1.1 requires unsigned long (uint32_t). Changed four call sites: two serialize_cdr writes and two deserialize_cdr reads. B. Public API error contract (deserialize_from_cdr always returned true) Both serialize_to_cdr() and deserialize_from_cdr() now wrap Fast-CDR calls in try/catch(eprosima::fastcdr::exception::Exception). Returns empty vector / false on error. Previously, exceptions escaped through the bool-returning function, crashing the receive thread on the first malformed payload. C. Sanity cap on CDR sequence lengths Added kMaxCdrSequenceElements = 1u << 20 (1,048,576). Before each vector::resize(seq_len) in the manual sequence helpers, throws BadParamException if seq_len exceeds the cap. Prevents OOM-abort on hostile or corrupt buffers (relevant because PR #9644's CycloneDDS path hands raw network bytes to deserialize_from_cdr via dds_takecdr). D. Explicit LITTLE_ENDIANNESS Replaced Cdr::DEFAULT_ENDIAN with Cdr::LITTLE_ENDIANNESS in serialize_to_cdr(), deserialize_from_cdr(), GenericCdrPubSubType::serialize(), and GenericCdrPubSubType::deserialize(). Hardcode payload->encapsulation = CDR_LE in GenericCdrPubSubType::serialize(). Matches DDSI-RTPS v2.5 Table 10.3 (CDR_LE = {0x00, 0x01}). In deserialize(), LITTLE_ENDIANNESS is the initial hint only; read_encapsulation() still parses the actual byte order from the wire header. --- Large-message publish fix --- E. Fix FastDDSPublisherMiddleware::Publish returning RETCODE_ERROR (code 1) for Image and PointCloud2 topics. Root cause: getSerializedSizeProvider() returned the static CdrTopicInfo::max_serialized_size() value (648 bytes for Image, 27597 bytes for PointCloud2). FastDDS pre-allocates the payload buffer at that size. serialize() then wrapped payload->data in a fixed-size FastBuffer and called serialize_cdr(), which threw NotEnoughMemoryException once the real payload (~1.4 MB for 800x600 RGB, 1-20 MB for LiDAR) exceeded the tiny pre-allocated buffer. RETCODE_ERROR propagated up to FastDDSPublisherMiddleware::Publish. Fix: - Add cdr_serialized_size(msg) to CdrSerialization.h: performs a dry-run serialize_encapsulation() + serialize_cdr() into a heap FastBuffer and returns getSerializedDataLength(). Result is provably equal to serialize_to_cdr(msg).size(). - Change getSerializedSizeProvider() in GenericCdrPubSubType to capture the message pointer and return cdr_serialized_size(*msg), so FastDDS resizes payload->data to the actual size before calling serialize(). - Change serialize() to use an auto-growing FastBuffer (no size ceiling) and memcpy into payload->data after serialization. The post-copy len > payload->max_size guard is a safety net for non-resizing memory modes. - Update CdrTopicInfo::max_serialized_size() doc comment: it is now an initial preallocation hint, not a hard upper bound. --- New tests (Group 10: 12 -> 21 tests) --- Spec-compliance tests (6): - pointcloud2_multi_field_round_trip: 3 PointFields, exercises manual sequence loop - tfmessage_multi_transform_round_trip: 2 TransformStamped entries - deserialize_truncated_returns_false: half-buffer Header, expects false - deserialize_corrupt_encapsulation_returns_false: 2-byte garbage header, expects false - deserialize_pointcloud2_hostile_length_returns_false: 0xFFFFFFFF fields_size, expects false - deserialize_tfmessage_hostile_length_returns_false: 0xFFFFFFFF transforms_size, expects false Large-message regression tests (3): - cdr_serialized_size_matches_serialize_to_cdr: asserts cdr_serialized_size(msg) == serialize_to_cdr(msg).size() for Header, Image, PointCloud2, TFMessage - cdr_serialized_size_image_exceeds_static_max: 800x600 RGB (~1.44 MB) round-trip; asserts computed size > CdrTopicInfo::max_serialized_size() (648 B) - cdr_serialized_size_pointcloud2_exceeds_static_max: 22000-point cloud (~352 KB) round-trip; same assertion for PointCloud2 (27597 B static max) Server tests: 117 -> 126. Client tests: 56 (unchanged). --- .../ros2/dds/fastdds/GenericCdrPubSubType.h | 50 ++-- .../carla/ros2/types/CdrSerialization.h | 86 +++++- .../source/carla/ros2/types/CdrTopicInfo.h | 11 +- .../test/server/test_dds_middleware.cpp | 263 +++++++++++++++++- 4 files changed, 373 insertions(+), 37 deletions(-) diff --git a/LibCarla/source/carla/ros2/dds/fastdds/GenericCdrPubSubType.h b/LibCarla/source/carla/ros2/dds/fastdds/GenericCdrPubSubType.h index 1bbc3382a2c..13bb6973f7c 100644 --- a/LibCarla/source/carla/ros2/dds/fastdds/GenericCdrPubSubType.h +++ b/LibCarla/source/carla/ros2/dds/fastdds/GenericCdrPubSubType.h @@ -13,6 +13,7 @@ #include #include +#include #include namespace carla { @@ -46,22 +47,28 @@ class GenericCdrPubSubType : public eprosima::fastdds::dds::TopicDataType { ~GenericCdrPubSubType() override = default; - /// Serialize a MsgType instance into the pre-allocated FastDDS payload buffer. + /// 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); - eprosima::fastcdr::FastBuffer fastbuffer( - reinterpret_cast(payload->data), - static_cast(payload->max_size)); + // 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( - fastbuffer, - eprosima::fastcdr::Cdr::DEFAULT_ENDIAN, + fb, + eprosima::fastcdr::Cdr::LITTLE_ENDIANNESS, eprosima::fastcdr::Cdr::DDS_CDR); - payload->encapsulation = (ser.endianness() == - eprosima::fastcdr::Cdr::BIG_ENDIANNESS) ? CDR_BE : CDR_LE; + payload->encapsulation = CDR_LE; try { ser.serialize_encapsulation(); @@ -70,7 +77,12 @@ class GenericCdrPubSubType : public eprosima::fastdds::dds::TopicDataType { return false; } - payload->length = static_cast(ser.getSerializedDataLength()); + 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; } @@ -84,9 +96,13 @@ class GenericCdrPubSubType : public eprosima::fastdds::dds::TopicDataType { 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::DEFAULT_ENDIAN, + eprosima::fastcdr::Cdr::LITTLE_ENDIANNESS, eprosima::fastcdr::Cdr::DDS_CDR); try { @@ -101,12 +117,14 @@ class GenericCdrPubSubType : public eprosima::fastdds::dds::TopicDataType { return true; } - /// Return a function that gives the CDR-serialized size for the given instance. - /// FastDDS uses this to size the payload buffer before calling serialize(). - std::function getSerializedSizeProvider(void* /*data*/) override { - return []() -> uint32_t { - return static_cast( - CdrTopicInfo::max_serialized_size()) + 4u; + /// 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); }; } diff --git a/LibCarla/source/carla/ros2/types/CdrSerialization.h b/LibCarla/source/carla/ros2/types/CdrSerialization.h index 2c072a2554f..dcc90801651 100644 --- a/LibCarla/source/carla/ros2/types/CdrSerialization.h +++ b/LibCarla/source/carla/ros2/types/CdrSerialization.h @@ -6,6 +6,8 @@ #include #include +#include +#include #include #include @@ -49,8 +51,21 @@ 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) // -------------------------------------------------------------------------- @@ -560,7 +575,8 @@ inline void serialize_cdr( serialize_cdr(cdr, m.header); cdr << m.height; cdr << m.width; - cdr << static_cast(m.fields.size()); + // 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); } @@ -576,8 +592,12 @@ inline void deserialize_cdr( deserialize_cdr(cdr, m.header); cdr >> m.height; cdr >> m.width; - int32_t fields_size{0}; + 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); @@ -595,7 +615,8 @@ inline void deserialize_cdr( /// Write length + elements manually. inline void serialize_cdr( eprosima::fastcdr::Cdr& cdr, const msg::TFMessage& m) { - cdr << static_cast(m.transforms.size()); + // 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); } @@ -603,8 +624,12 @@ inline void serialize_cdr( inline void deserialize_cdr( eprosima::fastcdr::Cdr& cdr, msg::TFMessage& m) { - int32_t transforms_size{0}; + 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); @@ -616,18 +641,27 @@ inline void deserialize_cdr( // ========================================================================== /// Serialize a msg::X to a CDR byte buffer including the DDS encapsulation -/// header (XCDR1 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(). +/// 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::DEFAULT_ENDIAN, + eprosima::fastcdr::Cdr::LITTLE_ENDIANNESS, eprosima::fastcdr::Cdr::DDS_CDR}; - cdr.serialize_encapsulation(); - serialize_cdr(cdr, msg); + 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{ @@ -635,10 +669,28 @@ std::vector serialize_to_cdr(const T& msg) { 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. The buffer must include the DDS encapsulation -/// header. +/// 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) { @@ -648,10 +700,14 @@ bool deserialize_from_cdr( size}; eprosima::fastcdr::Cdr cdr{ fb, - eprosima::fastcdr::Cdr::DEFAULT_ENDIAN, + eprosima::fastcdr::Cdr::LITTLE_ENDIANNESS, eprosima::fastcdr::Cdr::DDS_CDR}; - cdr.read_encapsulation(); - deserialize_cdr(cdr, msg); + try { + cdr.read_encapsulation(); + deserialize_cdr(cdr, msg); + } catch (const eprosima::fastcdr::exception::Exception&) { + return false; + } return true; } diff --git a/LibCarla/source/carla/ros2/types/CdrTopicInfo.h b/LibCarla/source/carla/ros2/types/CdrTopicInfo.h index e802e7215a7..1b9b10c1ce7 100644 --- a/LibCarla/source/carla/ros2/types/CdrTopicInfo.h +++ b/LibCarla/source/carla/ros2/types/CdrTopicInfo.h @@ -47,12 +47,13 @@ namespace ros2 { /// registering the type with a DomainParticipant. /// Follows the "pkg::msg::dds_::TypeName_" pattern. /// -/// max_serialized_size() — Conservative upper bound on the CDR payload size +/// max_serialized_size() — Initial preallocation hint for the CDR payload size /// in bytes, excluding the 4-byte DDS encapsulation -/// header. Used to pre-allocate topic buffers. -/// For types with variable-length fields (strings, -/// vectors) this is a practical maximum, not -/// an absolute one. +/// 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. diff --git a/LibCarla/source/test/server/test_dds_middleware.cpp b/LibCarla/source/test/server/test_dds_middleware.cpp index 53b5cdc06cb..22ccce3e2fe 100644 --- a/LibCarla/source/test/server/test_dds_middleware.cpp +++ b/LibCarla/source/test/server/test_dds_middleware.cpp @@ -581,7 +581,7 @@ TEST(cdr_topic_info, max_sizes_are_positive) { } // ========================================================================== -// Group 10: cdr_serialization (12 tests) +// Group 10: cdr_serialization (21 tests) // ========================================================================== TEST(cdr_serialization, time_round_trip) { @@ -840,6 +840,267 @@ TEST(cdr_serialization, empty_tfmessage_round_trip) { 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); +} + // ========================================================================== // Group 11: generic_cdr_pubsubtype (6 tests) // Tests for GenericCdrPubSubType — the single FastDDS TopicDataType