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