From 0ab35ee687b5c3f8d2d7972577867230bf679813 Mon Sep 17 00:00:00 2001 From: Jesus Armando Anaya Date: Fri, 27 Mar 2026 01:58:10 -0700 Subject: [PATCH 1/4] feat(LibCarla/ros2): add DDS middleware abstraction layer for multi-backend support MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Introduce a strategy-pattern abstraction that decouples PublisherImpl and SubscriberImpl from the FastDDS API, enabling future integration of additional DDS middleware implementations (e.g. CycloneDDS). New abstraction layer (LibCarla/source/carla/ros2/dds/): - DDSMiddleware enum and string conversion utilities - IDDSPublisherMiddleware / IDDSSubscriberMiddleware pure virtual interfaces - DDSMiddlewareFactory with thread-safe middleware selection and creation - FastDDSPublisherMiddleware / FastDDSSubscriberMiddleware template implementations behind the new interfaces - FastDDSTypeMap identity-mapping type traits (to be replaced with real conversions when POD message types are introduced) Modified files: - PublisherImpl.h: replaced direct FastDDS inheritance and members with delegation to IDDSPublisherMiddleware via the factory (public API unchanged) - SubscriberImpl.h: same pattern for subscriber side - cmake/fast_dds/CMakeLists.txt: added CARLA_ROS2_DDS_FASTDDS compile definition, new header globs, and fixed missing include directories in the debug build target No behavior change — FastDDS remains the only compiled middleware. Concrete publishers and subscribers are unaffected. --- LibCarla/cmake/fast_dds/CMakeLists.txt | 7 + .../source/carla/ros2/dds/DDSMiddleware.h | 56 +++++++ .../carla/ros2/dds/DDSMiddlewareFactory.h | 119 ++++++++++++++ .../carla/ros2/dds/IDDSPublisherMiddleware.h | 37 +++++ .../carla/ros2/dds/IDDSSubscriberMiddleware.h | 38 +++++ .../dds/fastdds/FastDDSPublisherMiddleware.h | 144 +++++++++++++++++ .../dds/fastdds/FastDDSSubscriberMiddleware.h | 151 ++++++++++++++++++ .../carla/ros2/dds/fastdds/FastDDSTypeMap.h | 96 +++++++++++ .../carla/ros2/publishers/PublisherImpl.h | 115 +++---------- .../carla/ros2/subscribers/SubscriberImpl.h | 124 ++------------ 10 files changed, 684 insertions(+), 203 deletions(-) create mode 100644 LibCarla/source/carla/ros2/dds/DDSMiddleware.h create mode 100644 LibCarla/source/carla/ros2/dds/DDSMiddlewareFactory.h create mode 100644 LibCarla/source/carla/ros2/dds/IDDSPublisherMiddleware.h create mode 100644 LibCarla/source/carla/ros2/dds/IDDSSubscriberMiddleware.h create mode 100644 LibCarla/source/carla/ros2/dds/fastdds/FastDDSPublisherMiddleware.h create mode 100644 LibCarla/source/carla/ros2/dds/fastdds/FastDDSSubscriberMiddleware.h create mode 100644 LibCarla/source/carla/ros2/dds/fastdds/FastDDSTypeMap.h diff --git a/LibCarla/cmake/fast_dds/CMakeLists.txt b/LibCarla/cmake/fast_dds/CMakeLists.txt index 366549eefaf..4ff74c21d57 100644 --- a/LibCarla/cmake/fast_dds/CMakeLists.txt +++ b/LibCarla/cmake/fast_dds/CMakeLists.txt @@ -9,6 +9,8 @@ file(GLOB libcarla_carla_fastdds_headers "${libcarla_source_path}/carla/ros2/subscribers/*.h" "${libcarla_source_path}/carla/ros2/listeners/*.h" "${libcarla_source_path}/carla/ros2/types/*.h" + "${libcarla_source_path}/carla/ros2/dds/*.h" + "${libcarla_source_path}/carla/ros2/dds/fastdds/*.h" ) install(FILES ${libcarla_carla_fastdds_headers} DESTINATION include/carla/ros2) @@ -30,6 +32,7 @@ file(GLOB libcarla_fastdds_sources if (LIBCARLA_BUILD_RELEASE) add_library(carla_fastdds STATIC ${libcarla_fastdds_sources}) + target_compile_definitions(carla_fastdds PRIVATE CARLA_ROS2_DDS_FASTDDS) target_compile_options(carla_fastdds PRIVATE -fexceptions) target_include_directories(carla_fastdds SYSTEM PRIVATE @@ -48,11 +51,15 @@ if (LIBCARLA_BUILD_DEBUG) add_library(carla_fastdds_debug STATIC ${libcarla_fastdds_sources}) + target_compile_definitions(carla_fastdds_debug PRIVATE CARLA_ROS2_DDS_FASTDDS) target_compile_options(carla_fastdds_debug PRIVATE -fexceptions) target_include_directories(carla_fastdds_debug SYSTEM PRIVATE "${BOOST_INCLUDE_PATH}" "${RPCLIB_INCLUDE_PATH}") + + target_include_directories(carla_fastdds_debug PRIVATE "${FASTDDS_INCLUDE_PATH}") + target_include_directories(carla_fastdds_debug PRIVATE "${libcarla_source_path}/carla/ros2") install(TARGETS carla_fastdds_debug DESTINATION lib) set_target_properties(carla_fastdds_debug PROPERTIES COMPILE_FLAGS "${CMAKE_CXX_FLAGS_DEBUG}") target_compile_definitions(carla_fastdds_debug PUBLIC -DBOOST_ASIO_ENABLE_BUFFER_DEBUGGING) diff --git a/LibCarla/source/carla/ros2/dds/DDSMiddleware.h b/LibCarla/source/carla/ros2/dds/DDSMiddleware.h new file mode 100644 index 00000000000..35a4d89be47 --- /dev/null +++ b/LibCarla/source/carla/ros2/dds/DDSMiddleware.h @@ -0,0 +1,56 @@ +// Copyright (c) 2025 Computer Vision Center (CVC) at the Universitat Autonoma de Barcelona (UAB). +// This work is licensed under the terms of the MIT license. +// For a copy, see . + +#pragma once + +#include + +namespace carla { +namespace ros2 { + +/// Enumeration of available DDS middleware implementations. +/// Passed to ROS2::Enable() to select the middleware at startup. +/// Once set, the middleware cannot be changed without restarting. +enum class DDSMiddleware { + FastDDS +}; + +/// Convert a DDSMiddleware enum value to a readable string. +inline const char* DDSMiddlewareToString(DDSMiddleware middleware) { + switch (middleware) { + case DDSMiddleware::FastDDS: + return "FastDDS"; + } + return "Unknown"; +} + +/// Result of parsing a middleware name string. +struct DDSMiddlewareParseResult { + bool valid; + DDSMiddleware middleware; +}; + +/// Parse a middleware name string (lowercase). Returns {true, middleware} on match, +/// {false, FastDDS} for unrecognized values. +inline DDSMiddlewareParseResult DDSMiddlewareFromString(const std::string& name) { + if (name == "fastdds") { + return {true, DDSMiddleware::FastDDS}; + } + return {false, DDSMiddleware::FastDDS}; +} + +/// Return a readable list of middleware implementations compiled into this binary. +inline std::string GetAvailableMiddlewareString() { + std::string result; +#if defined(CARLA_ROS2_DDS_FASTDDS) + result += "FastDDS"; +#endif + if (result.empty()) { + result = "none"; + } + return result; +} + +} // namespace ros2 +} // namespace carla diff --git a/LibCarla/source/carla/ros2/dds/DDSMiddlewareFactory.h b/LibCarla/source/carla/ros2/dds/DDSMiddlewareFactory.h new file mode 100644 index 00000000000..5b7b2adcd9a --- /dev/null +++ b/LibCarla/source/carla/ros2/dds/DDSMiddlewareFactory.h @@ -0,0 +1,119 @@ +// Copyright (c) 2025 Computer Vision Center (CVC) at the Universitat Autonoma de Barcelona (UAB). +// This work is licensed under the terms of the MIT license. +// For a copy, see . + +#pragma once + +#include +#include + +#include "carla/ros2/dds/DDSMiddleware.h" +#include "carla/ros2/dds/IDDSPublisherMiddleware.h" +#include "carla/ros2/dds/IDDSSubscriberMiddleware.h" +#include "carla/Logging.h" + +#if defined(CARLA_ROS2_DDS_FASTDDS) +# include "carla/ros2/dds/fastdds/FastDDSPublisherMiddleware.h" +# include "carla/ros2/dds/fastdds/FastDDSSubscriberMiddleware.h" +#endif + +namespace carla { +namespace ros2 { + +/// Factory that creates DDS publisher/subscriber middleware based on the active middleware selection. +/// The middleware is set once at startup via SetMiddleware() before any DDS entities are created. +/// After the first entity is created, changing the middleware has undefined behavior. +class DDSMiddlewareFactory { + public: + /// Select the DDS middleware for all subsequent publisher/subscriber creation. + /// Must be called before any publisher or subscriber is initialized. + static void SetMiddleware(DDSMiddleware middleware) { + GetActiveMiddleware() = middleware; + } + + /// @return The currently selected DDS middleware. + static DDSMiddleware GetMiddleware() { + return GetActiveMiddleware(); + } + + /// Check whether a specific middleware was compiled into this binary. + static bool IsMiddlewareAvailable(DDSMiddleware middleware) { + switch (middleware) { + case DDSMiddleware::FastDDS: +#if defined(CARLA_ROS2_DDS_FASTDDS) + return true; +#else + return false; +#endif + } + return false; + } + + /// Result of middleware resolution — whether resolution succeeded and which middleware to use. + struct MiddlewareResolution { + bool success; + DDSMiddleware middleware; + }; + + /// Resolve the requested middleware strictly — no fallback to other middleware. + /// Returns {true, requested} if available, {false, requested} otherwise. + static MiddlewareResolution ResolveMiddleware(DDSMiddleware requested) { + if (IsMiddlewareAvailable(requested)) { + return {true, requested}; + } + return {false, requested}; + } + + /// Return a readable list of middleware implementations compiled into this binary. + /// Delegates to the free function in DDSMiddleware.h. + static std::string GetAvailableMiddlewareString() { + return carla::ros2::GetAvailableMiddlewareString(); + } + + /// Create a publisher middleware for traits type T. + /// T must provide: + /// T::msg_type — the message type + template + static std::unique_ptr CreatePublisher() { + switch (GetActiveMiddleware()) { + case DDSMiddleware::FastDDS: +#if defined(CARLA_ROS2_DDS_FASTDDS) + return std::unique_ptr( + new FastDDSPublisherMiddleware()); +#else + log_error("DDSMiddlewareFactory: FastDDS not compiled in"); + return nullptr; +#endif + } + return nullptr; + } + + /// Create a subscriber middleware for traits type S. + /// S must provide: + /// S::msg_type — the message type + template + static std::unique_ptr CreateSubscriber() { + switch (GetActiveMiddleware()) { + case DDSMiddleware::FastDDS: +#if defined(CARLA_ROS2_DDS_FASTDDS) + return std::unique_ptr( + new FastDDSSubscriberMiddleware()); +#else + log_error("DDSMiddlewareFactory: FastDDS not compiled in"); + return nullptr; +#endif + } + return nullptr; + } + + private: + /// Returns reference to the active middleware selection + /// (function-local static for C++11 thread safety). + static DDSMiddleware& GetActiveMiddleware() { + static DDSMiddleware active_middleware = DDSMiddleware::FastDDS; + return active_middleware; + } +}; + +} // namespace ros2 +} // namespace carla diff --git a/LibCarla/source/carla/ros2/dds/IDDSPublisherMiddleware.h b/LibCarla/source/carla/ros2/dds/IDDSPublisherMiddleware.h new file mode 100644 index 00000000000..4e757df0c67 --- /dev/null +++ b/LibCarla/source/carla/ros2/dds/IDDSPublisherMiddleware.h @@ -0,0 +1,37 @@ +// Copyright (c) 2025 Computer Vision Center (CVC) at the Universitat Autonoma de Barcelona (UAB). +// This work is licensed under the terms of the MIT license. +// For a copy, see . + +#pragma once + +#include + +namespace carla { +namespace ros2 { + +/// Type-erased abstract interface for a DDS publisher middleware. +/// Concrete implementations handle all vendor-specific DDS entity creation, +/// type registration, and data writing. +class IDDSPublisherMiddleware { + public: + virtual ~IDDSPublisherMiddleware() = default; + + /// Initialize DDS entities (participant, publisher, topic, writer). + /// @param topic_name Full DDS topic name including "rt/" prefix. + /// @return true on success. + virtual bool Init(const std::string& topic_name) = 0; + + /// Serialize and write a message to the DDS network. + /// @param message_data Pointer to the message object (type-erased, cast internally). + /// @return true if the write succeeded. + virtual bool Publish(void* message_data) = 0; + + /// @return true if at least one subscriber is matched. + virtual bool IsAlive() const = 0; + + /// @return The DDS topic name this publisher is bound to. + virtual std::string GetTopicName() const = 0; +}; + +} // namespace ros2 +} // namespace carla diff --git a/LibCarla/source/carla/ros2/dds/IDDSSubscriberMiddleware.h b/LibCarla/source/carla/ros2/dds/IDDSSubscriberMiddleware.h new file mode 100644 index 00000000000..896bc86295d --- /dev/null +++ b/LibCarla/source/carla/ros2/dds/IDDSSubscriberMiddleware.h @@ -0,0 +1,38 @@ +// Copyright (c) 2025 Computer Vision Center (CVC) at the Universitat Autonoma de Barcelona (UAB). +// This work is licensed under the terms of the MIT license. +// For a copy, see . + +#pragma once + +#include + +namespace carla { +namespace ros2 { + +/// Type-erased abstract interface for a DDS subscriber middleware. +/// Concrete implementations write received messages directly into the caller-provided +/// storage (message_ptr / new_message_flag) to avoid an extra copy. +class IDDSSubscriberMiddleware { + public: + virtual ~IDDSSubscriberMiddleware() = default; + + /// Initialize DDS entities (participant, subscriber, topic, reader). + /// The middleware writes incoming messages to *message_ptr and sets *new_message_flag = true. + /// @param topic_name Full DDS topic name. + /// @param message_ptr Pointer to the message storage owned by SubscriberImpl. + /// @param new_message_flag Pointer to the new-message flag owned by SubscriberImpl. + /// @return true on success. + virtual bool Init( + const std::string& topic_name, + void* message_ptr, + bool* new_message_flag) = 0; + + /// @return true if at least one publisher is matched. + virtual bool IsAlive() const = 0; + + /// @return The DDS topic name this subscriber is bound to. + virtual std::string GetTopicName() const = 0; +}; + +} // namespace ros2 +} // namespace carla diff --git a/LibCarla/source/carla/ros2/dds/fastdds/FastDDSPublisherMiddleware.h b/LibCarla/source/carla/ros2/dds/fastdds/FastDDSPublisherMiddleware.h new file mode 100644 index 00000000000..07ffec6bbaf --- /dev/null +++ b/LibCarla/source/carla/ros2/dds/fastdds/FastDDSPublisherMiddleware.h @@ -0,0 +1,144 @@ +// Copyright (c) 2025 Computer Vision Center (CVC) at the Universitat Autonoma de Barcelona (UAB). +// This work is licensed under the terms of the MIT license. +// For a copy, see . + +#pragma once + +#include "carla/ros2/dds/IDDSPublisherMiddleware.h" +#include "carla/ros2/dds/fastdds/FastDDSTypeMap.h" +#include "carla/Logging.h" + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace carla { +namespace ros2 { + +namespace efd = eprosima::fastdds::dds; +using erc = eprosima::fastrtps::types::ReturnCode_t; + +/// FastDDS implementation of IDDSPublisherMiddleware. +/// Parameterized on a traits type T that provides: +/// T::msg_type — the message type +/// Native FastDDS types are resolved via FastDDSTypeMap. +template +class FastDDSPublisherMiddleware + : public IDDSPublisherMiddleware, + public eprosima::fastdds::dds::DataWriterListener { + public: + using msg_type = typename T::msg_type; + using type_map = FastDDSTypeMap; + using fastdds_type = typename type_map::fastdds_type; + using fastdds_pubsub_type = typename type_map::fastdds_pubsub_type; + + void on_publication_matched( + efd::DataWriter* writer, + const efd::PublicationMatchedStatus& info) override { + _alive = (info.total_count > 0); + } + + ~FastDDSPublisherMiddleware() override { + if (_datawriter) { + _publisher->delete_datawriter(_datawriter); + } + if (_publisher) { + _participant->delete_publisher(_publisher); + } + if (_topic) { + _participant->delete_topic(_topic); + } + if (_participant) { + efd::DomainParticipantFactory::get_instance()->delete_participant(_participant); + } + } + + bool Init(const std::string& topic_name) override { + if (_type == nullptr) { + log_error("FastDDSPublisherMiddleware: Invalid TypeSupport"); + return false; + } + + efd::DomainParticipantQos pqos = efd::PARTICIPANT_QOS_DEFAULT; + auto factory = efd::DomainParticipantFactory::get_instance(); + _participant = factory->create_participant(0, pqos); + if (_participant == nullptr) { + log_error("FastDDSPublisherMiddleware: Failed to create DomainParticipant"); + return false; + } + _type.register_type(_participant); + + efd::PublisherQos pubqos = efd::PUBLISHER_QOS_DEFAULT; + _publisher = _participant->create_publisher(pubqos, nullptr); + if (_publisher == nullptr) { + log_error("FastDDSPublisherMiddleware: Failed to create Publisher"); + return false; + } + + efd::TopicQos tqos = efd::TOPIC_QOS_DEFAULT; + _topic = _participant->create_topic(topic_name, _type->getName(), tqos); + if (_topic == nullptr) { + log_error("FastDDSPublisherMiddleware: Failed to create Topic"); + return false; + } + + efd::DataWriterQos wqos = efd::DATAWRITER_QOS_DEFAULT; + wqos.endpoint().history_memory_policy = + eprosima::fastrtps::rtps::PREALLOCATED_WITH_REALLOC_MEMORY_MODE; + efd::DataWriterListener* listener = + static_cast(this); + _datawriter = _publisher->create_datawriter(_topic, wqos, listener); + if (_datawriter == nullptr) { + log_error("FastDDSPublisherMiddleware: Failed to create DataWriter"); + return false; + } + + _topic_name = topic_name; + return true; + } + + bool Publish(void* message_data) override { + auto* msg = static_cast(message_data); + to_fastdds(*msg, _fastdds_msg); + eprosima::fastrtps::rtps::InstanceHandle_t instance_handle; + erc rcode = _datawriter->write(&_fastdds_msg, instance_handle); + if (rcode == erc::ReturnCodeValue::RETCODE_OK) { + return true; + } + log_error("FastDDSPublisherMiddleware::Publish (", + _topic_name, ") failed with code:", rcode()); + return false; + } + + bool IsAlive() const override { + return _alive; + } + + std::string GetTopicName() const override { + return _topic_name; + } + + private: + efd::DomainParticipant* _participant { nullptr }; + efd::Publisher* _publisher { nullptr }; + efd::Topic* _topic { nullptr }; + efd::DataWriter* _datawriter { nullptr }; + efd::TypeSupport _type { new fastdds_pubsub_type() }; + + fastdds_type _fastdds_msg; + std::string _topic_name; + bool _alive { false }; +}; + +} // namespace ros2 +} // namespace carla diff --git a/LibCarla/source/carla/ros2/dds/fastdds/FastDDSSubscriberMiddleware.h b/LibCarla/source/carla/ros2/dds/fastdds/FastDDSSubscriberMiddleware.h new file mode 100644 index 00000000000..03f50f757ba --- /dev/null +++ b/LibCarla/source/carla/ros2/dds/fastdds/FastDDSSubscriberMiddleware.h @@ -0,0 +1,151 @@ +// Copyright (c) 2025 Computer Vision Center (CVC) at the Universitat Autonoma de Barcelona (UAB). +// This work is licensed under the terms of the MIT license. +// For a copy, see . + +#pragma once + +#include "carla/ros2/dds/IDDSSubscriberMiddleware.h" +#include "carla/ros2/dds/fastdds/FastDDSTypeMap.h" +#include "carla/Logging.h" + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace carla { +namespace ros2 { + +namespace efd = eprosima::fastdds::dds; +using erc = eprosima::fastrtps::types::ReturnCode_t; + +/// FastDDS implementation of IDDSSubscriberMiddleware. +/// Parameterized on traits type S that provides: +/// S::msg_type — the message type +/// Native FastDDS types are resolved via FastDDSTypeMap. +template +class FastDDSSubscriberMiddleware + : public IDDSSubscriberMiddleware, + public eprosima::fastdds::dds::DataReaderListener { + public: + using msg_type = typename S::msg_type; + using type_map = FastDDSTypeMap; + using fastdds_type = typename type_map::fastdds_type; + using fastdds_pubsub_type = typename type_map::fastdds_pubsub_type; + + void on_subscription_matched( + efd::DataReader* reader, + const efd::SubscriptionMatchedStatus& info) override { + _alive = (info.total_count > 0); + } + + void on_data_available(efd::DataReader* reader) override { + efd::SampleInfo info; + erc rcode = reader->take_next_sample(&_fastdds_msg, &info); + if (rcode == erc::ReturnCodeValue::RETCODE_OK) { + from_fastdds(_fastdds_msg, *_message_ptr); + *_new_message_ptr = true; + } else { + log_error("FastDDSSubscriberMiddleware::on_data_available (", + _topic_name, ") failed with code:", rcode()); + } + } + + ~FastDDSSubscriberMiddleware() override { + if (_datareader) { + _subscriber->delete_datareader(_datareader); + } + if (_subscriber) { + _participant->delete_subscriber(_subscriber); + } + if (_topic) { + _participant->delete_topic(_topic); + } + if (_participant) { + efd::DomainParticipantFactory::get_instance()->delete_participant(_participant); + } + } + + bool Init( + const std::string& topic_name, + void* message_ptr, + bool* new_message_flag) override { + _message_ptr = static_cast(message_ptr); + _new_message_ptr = new_message_flag; + + if (_type == nullptr) { + log_error("FastDDSSubscriberMiddleware: Invalid TypeSupport"); + return false; + } + + efd::DomainParticipantQos pqos = efd::PARTICIPANT_QOS_DEFAULT; + auto factory = efd::DomainParticipantFactory::get_instance(); + _participant = factory->create_participant(0, pqos); + if (_participant == nullptr) { + log_error("FastDDSSubscriberMiddleware: Failed to create DomainParticipant"); + return false; + } + _type.register_type(_participant); + + efd::SubscriberQos subqos = efd::SUBSCRIBER_QOS_DEFAULT; + _subscriber = _participant->create_subscriber(subqos, nullptr); + if (_subscriber == nullptr) { + log_error("FastDDSSubscriberMiddleware: Failed to create Subscriber"); + return false; + } + + efd::TopicQos tqos = efd::TOPIC_QOS_DEFAULT; + _topic = _participant->create_topic(topic_name, _type->getName(), tqos); + if (_topic == nullptr) { + log_error("FastDDSSubscriberMiddleware: Failed to create Topic"); + return false; + } + + efd::DataReaderQos rqos = efd::DATAREADER_QOS_DEFAULT; + efd::DataReaderListener* listener = + static_cast(this); + _datareader = _subscriber->create_datareader(_topic, rqos, listener); + if (_datareader == nullptr) { + log_error("FastDDSSubscriberMiddleware: Failed to create DataReader"); + return false; + } + + _topic_name = topic_name; + return true; + } + + bool IsAlive() const override { + return _alive; + } + + std::string GetTopicName() const override { + return _topic_name; + } + + private: + efd::DomainParticipant* _participant { nullptr }; + efd::Subscriber* _subscriber { nullptr }; + efd::Topic* _topic { nullptr }; + efd::DataReader* _datareader { nullptr }; + efd::TypeSupport _type { new fastdds_pubsub_type() }; + + fastdds_type _fastdds_msg; + msg_type* _message_ptr { nullptr }; + bool* _new_message_ptr { nullptr }; + + std::string _topic_name; + bool _alive { false }; +}; + +} // namespace ros2 +} // namespace carla diff --git a/LibCarla/source/carla/ros2/dds/fastdds/FastDDSTypeMap.h b/LibCarla/source/carla/ros2/dds/fastdds/FastDDSTypeMap.h new file mode 100644 index 00000000000..7a292f3a2b1 --- /dev/null +++ b/LibCarla/source/carla/ros2/dds/fastdds/FastDDSTypeMap.h @@ -0,0 +1,96 @@ +// Copyright (c) 2025 Computer Vision Center (CVC) at the Universitat Autonoma de Barcelona (UAB). +// This work is licensed under the terms of the MIT license. +// For a copy, see . + +// Identity-mapping version: msg_type IS the FastDDS type, so each +// specialization maps a type to itself. + +#pragma once + +// PubSubType headers for type registration +#include "carla/ros2/types/NavSatFixPubSubTypes.h" +#include "carla/ros2/types/ImagePubSubTypes.h" +#include "carla/ros2/types/CameraInfoPubSubTypes.h" +#include "carla/ros2/types/ImuPubSubTypes.h" +#include "carla/ros2/types/PointCloud2PubSubTypes.h" +#include "carla/ros2/types/ClockPubSubTypes.h" +#include "carla/ros2/types/TFMessagePubSubTypes.h" +#include "carla/ros2/types/CarlaCollisionEventPubSubTypes.h" +#include "carla/ros2/types/CarlaEgoVehicleControlPubSubTypes.h" +#include "carla/ros2/types/AckermannDriveStampedPubSubTypes.h" + +namespace carla { +namespace ros2 { + +/// Maps a message type to its FastDDS native type and PubSubType. +/// Primary template is intentionally undefined — only specializations are valid. +template struct FastDDSTypeMap; + +// --- Identity specializations --- +// In the current codebase, msg_type is already the FastDDS-generated type. +// Each specialization maps the type to itself and its corresponding PubSubType. + +template<> struct FastDDSTypeMap { + using fastdds_type = sensor_msgs::msg::NavSatFix; + using fastdds_pubsub_type = sensor_msgs::msg::NavSatFixPubSubType; +}; + +template<> struct FastDDSTypeMap { + using fastdds_type = sensor_msgs::msg::Image; + using fastdds_pubsub_type = sensor_msgs::msg::ImagePubSubType; +}; + +template<> struct FastDDSTypeMap { + using fastdds_type = sensor_msgs::msg::CameraInfo; + using fastdds_pubsub_type = sensor_msgs::msg::CameraInfoPubSubType; +}; + +template<> struct FastDDSTypeMap { + using fastdds_type = sensor_msgs::msg::Imu; + using fastdds_pubsub_type = sensor_msgs::msg::ImuPubSubType; +}; + +template<> struct FastDDSTypeMap { + using fastdds_type = sensor_msgs::msg::PointCloud2; + using fastdds_pubsub_type = sensor_msgs::msg::PointCloud2PubSubType; +}; + +template<> struct FastDDSTypeMap { + using fastdds_type = rosgraph::msg::Clock; + using fastdds_pubsub_type = rosgraph::msg::ClockPubSubType; +}; + +template<> struct FastDDSTypeMap { + using fastdds_type = tf2_msgs::msg::TFMessage; + using fastdds_pubsub_type = tf2_msgs::msg::TFMessagePubSubType; +}; + +template<> struct FastDDSTypeMap { + using fastdds_type = carla_msgs::msg::CarlaCollisionEvent; + using fastdds_pubsub_type = carla_msgs::msg::CarlaCollisionEventPubSubType; +}; + +template<> struct FastDDSTypeMap { + using fastdds_type = carla_msgs::msg::CarlaEgoVehicleControl; + using fastdds_pubsub_type = carla_msgs::msg::CarlaEgoVehicleControlPubSubType; +}; + +template<> struct FastDDSTypeMap { + using fastdds_type = ackermann_msgs::msg::AckermannDriveStamped; + using fastdds_pubsub_type = ackermann_msgs::msg::AckermannDriveStampedPubSubType; +}; + +/// Identity conversion: copy src to dst when both types are the same. +/// In the next phase, we will replace these with real POD-to-FastDDS conversions. +template +inline void to_fastdds(const T& src, T& dst) { + dst = src; +} + +template +inline void from_fastdds(const T& src, T& dst) { + dst = src; +} + +} // namespace ros2 +} // namespace carla diff --git a/LibCarla/source/carla/ros2/publishers/PublisherImpl.h b/LibCarla/source/carla/ros2/publishers/PublisherImpl.h index 377058822fe..9d46419eab7 100644 --- a/LibCarla/source/carla/ros2/publishers/PublisherImpl.h +++ b/LibCarla/source/carla/ros2/publishers/PublisherImpl.h @@ -5,112 +5,40 @@ #pragma once #include +#include -#include -#include -#include - -#include -#include - -#include -#include -#include -#include - -#include - -#include - -#include -#include - +#include "carla/ros2/dds/DDSMiddlewareFactory.h" #include "carla/Logging.h" namespace carla { namespace ros2 { - namespace efd = eprosima::fastdds::dds; - using erc = eprosima::fastrtps::types::ReturnCode_t; - template - class PublisherImpl : public eprosima::fastdds::dds::DataWriterListener { + class PublisherImpl { public: using msg_type = typename T::msg_type; - using msg_pubsub_type = typename T::msg_pubsub_type; - - efd::DomainParticipant* _participant { nullptr }; - efd::Publisher* _publisher { nullptr }; - efd::Topic* _topic { nullptr }; - efd::DataWriter* _datawriter { nullptr }; - efd::TypeSupport _type { new msg_pubsub_type() }; - - void on_publication_matched(efd::DataWriter* /*writer*/, const efd::PublicationMatchedStatus& info) override { - _alive = (info.total_count > 0) ? true : false; - } - - ~PublisherImpl() { - if (_datawriter) - _publisher->delete_datawriter(_datawriter); - - if (_publisher) - _participant->delete_publisher(_publisher); - - if (_topic) - _participant->delete_topic(_topic); - - if (_participant) - efd::DomainParticipantFactory::get_instance()->delete_participant(_participant); - } bool Init(std::string topic_name) { - if (_type == nullptr) { - log_error("Invalid TypeSupport"); + _middleware = DDSMiddlewareFactory::CreatePublisher(); + if (!_middleware) { + log_error("PublisherImpl: Failed to create middleware publisher"); return false; } - - efd::DomainParticipantQos pqos = efd::PARTICIPANT_QOS_DEFAULT; - auto factory = efd::DomainParticipantFactory::get_instance(); - _participant = factory->create_participant(0, pqos); - if (_participant == nullptr) { - log_error("Failed to create DomainParticipant"); - return false; - } - _type.register_type(_participant); - - efd::PublisherQos pubqos = efd::PUBLISHER_QOS_DEFAULT; - _publisher = _participant->create_publisher(pubqos, nullptr); - if (_publisher == nullptr) { - log_error("Failed to create Publisher"); - return false; - } - - efd::TopicQos tqos = efd::TOPIC_QOS_DEFAULT; - _topic = _participant->create_topic(topic_name, _type->getName(), tqos); - if (_topic == nullptr) { - log_error("Failed to create Topic"); - return false; - } - - efd::DataWriterQos wqos = efd::DATAWRITER_QOS_DEFAULT; - wqos.endpoint().history_memory_policy = eprosima::fastrtps::rtps::PREALLOCATED_WITH_REALLOC_MEMORY_MODE; - efd::DataWriterListener* listener = static_cast(this); - _datawriter = _publisher->create_datawriter(_topic, wqos, listener); - if (_datawriter == nullptr) { - std::cerr << "Failed to create DataWriter" << std::endl; - return false; - } - - _topic_name = topic_name; - return true; + return _middleware->Init(topic_name); } std::string GetTopicName() { - return _topic_name; + if (_middleware) { + return _middleware->GetTopicName(); + } + return ""; } bool IsAlive() { - return _alive; + if (_middleware) { + return _middleware->IsAlive(); + } + return false; } msg_type* GetMessage() { @@ -118,20 +46,15 @@ namespace ros2 { } bool Publish() { - eprosima::fastrtps::rtps::InstanceHandle_t instance_handle; - eprosima::fastrtps::types::ReturnCode_t rcode = _datawriter->write(&_message, instance_handle); - if (rcode == eprosima::fastrtps::types::ReturnCode_t::ReturnCodeValue::RETCODE_OK) { - return true; - } else { - log_error("PublisherImpl::Publish (", this->GetTopicName(), ") failed with code:", rcode()); + if (!_middleware) { + log_error("PublisherImpl::Publish called before Init"); return false; } + return _middleware->Publish(&_message); } private: - std::string _topic_name; - - bool _alive { false }; + std::unique_ptr _middleware; msg_type _message; }; diff --git a/LibCarla/source/carla/ros2/subscribers/SubscriberImpl.h b/LibCarla/source/carla/ros2/subscribers/SubscriberImpl.h index c9e489cc135..5d1f21ea5c5 100644 --- a/LibCarla/source/carla/ros2/subscribers/SubscriberImpl.h +++ b/LibCarla/source/carla/ros2/subscribers/SubscriberImpl.h @@ -5,129 +5,41 @@ #pragma once #include +#include #include "carla/ros2/subscribers/BaseSubscriber.h" - -#include -#include -#include - -#include -#include - -#include -#include -#include -#include -#include -#include - -#include - -#include -#include - +#include "carla/ros2/dds/DDSMiddlewareFactory.h" #include "carla/Logging.h" namespace carla { namespace ros2 { - namespace efd = eprosima::fastdds::dds; - using erc = eprosima::fastrtps::types::ReturnCode_t; - template - class SubscriberImpl : public eprosima::fastdds::dds::DataReaderListener { + class SubscriberImpl { public: using msg_type = typename S::msg_type; - using msg_pubsub_type = typename S::msg_pubsub_type; - - efd::DomainParticipant* _participant { nullptr }; - efd::Subscriber* _subscriber { nullptr }; - efd::Topic* _topic { nullptr }; - efd::DataReader* _datareader { nullptr }; - efd::TypeSupport _type { new msg_pubsub_type() }; - - void on_subscription_matched(efd::DataReader* /*reader*/, const efd::SubscriptionMatchedStatus& info) override { - _alive = (info.total_count > 0) ? true : false; - } - - void on_data_available(efd::DataReader* reader) override { - efd::SampleInfo info; - msg_type message; - - eprosima::fastrtps::types::ReturnCode_t rcode = reader->take_next_sample(&_message, &info); - if (rcode == eprosima::fastrtps::types::ReturnCode_t::ReturnCodeValue::RETCODE_OK) { - // TODO: Process messages directly. - _new_message = true; - } else { - log_error("SubscriberImpl::on_data_available (", this->GetTopicName(), ") failed with code:", rcode()); - } - } - - ~SubscriberImpl() { - if (_datareader) - _subscriber->delete_datareader(_datareader); - - if (_subscriber) - _participant->delete_subscriber(_subscriber); - - if (_topic) - _participant->delete_topic(_topic); - - if (_participant) - efd::DomainParticipantFactory::get_instance()->delete_participant(_participant); - } - // bool Init(std::string topic_name, S *subscriber) { bool Init(std::string topic_name) { - if (_type == nullptr) { - log_error("Invalid TypeSupport"); + _middleware = DDSMiddlewareFactory::CreateSubscriber(); + if (!_middleware) { + log_error("SubscriberImpl: Failed to create middleware subscriber"); return false; } - - efd::DomainParticipantQos pqos = efd::PARTICIPANT_QOS_DEFAULT; - auto factory = efd::DomainParticipantFactory::get_instance(); - _participant = factory->create_participant(0, pqos); - if (_participant == nullptr) { - log_error("Failed to create DomainParticipant"); - return false; - } - _type.register_type(_participant); - - efd::SubscriberQos subqos = efd::SUBSCRIBER_QOS_DEFAULT; - _subscriber = _participant->create_subscriber(subqos, nullptr); - if (_subscriber == nullptr) { - log_error("Failed to create Subscriber"); - return false; - } - - efd::TopicQos tqos = efd::TOPIC_QOS_DEFAULT; - _topic = _participant->create_topic(topic_name, _type->getName(), tqos); - if (_topic == nullptr) { - log_error("Failed to create Topic"); - return false; - } - - efd::DataReaderQos rqos = efd::DATAREADER_QOS_DEFAULT; - efd::DataReaderListener* listener = static_cast(this); - _datareader = _subscriber->create_datareader(_topic, rqos, listener); - if (_datareader == nullptr) { - log_error("Failed to create DataReader"); - return false; - } - - _topic_name = topic_name; - - // _subscriber = subscriber; - return true; + return _middleware->Init(topic_name, &_message, &_new_message); } std::string GetTopicName() { - return _topic_name; + if (_middleware) { + return _middleware->GetTopicName(); + } + return ""; } bool IsAlive() { - return _alive; + if (_middleware) { + return _middleware->IsAlive(); + } + return false; } msg_type GetMessage() { @@ -138,11 +50,9 @@ namespace ros2 { bool HasNewMessage() { return _new_message; } private: - std::string _topic_name; - - bool _alive { false }; - bool _new_message { false }; + std::unique_ptr _middleware; msg_type _message; + bool _new_message { false }; }; } // namespace ros2 From e665d2afc3c3cec94b45de3c8383d7b3192e74dd Mon Sep 17 00:00:00 2001 From: Jesus Armando Anaya Date: Fri, 27 Mar 2026 18:50:41 -0700 Subject: [PATCH 2/4] feat(LibCarla/ros2): add POD message types, FastDDS conversions, and real TypeMap specializations Introduce backend-neutral POD message structs and to_fastdds/from_fastdds conversion functions, completing the type abstraction layer started in PR #1 without touching any concrete publisher or subscriber. New files (LibCarla/source/carla/ros2/types/): - 31 plain C++ structs in types/msg/ - one per ROS2 message type, no DDS dependency, standard library headers only, all members value-initialized - FastDDSConversions.h: inline to_fastdds/from_fastdds overload pairs for all 31 type pairs, ordered bottom-up so composites call primitives; move overloads for msg::Image and msg::PointCloud2 to avoid copying large data vectors New test file (LibCarla/source/test/server/test_dds_middleware.cpp): - 45 unit tests across 10 groups covering the middleware enum, factory, interfaces, PublisherImpl, and SubscriberImpl using hand-written mocks; no DDS daemon required (CARLA_ROS2_DDS_TESTING suppresses real includes) Modified files: - FastDDSTypeMap.h: 28 real specializations mapping msg::X to FastDDS types, coexisting with the existing identity specializations (different key types, no ODR conflict) - DDSMiddleware.h: add ToROS2DDSTypeName() for use in PR #3 - DDSMiddlewareFactory.h: add CARLA_ROS2_DDS_TESTING guard to FastDDS include/create blocks so test binary compiles without real DDS headers - PublisherImpl.h / SubscriberImpl.h: LIBCARLA_WITH_GTEST test seams for middleware injection and message simulation - cmake/fast_dds/CMakeLists.txt: added glob for types/msg/*.h install No behavior change - identity TypeMap specializations remain active; all concrete publishers and subscribers compile and work unchanged. --- LibCarla/cmake/fast_dds/CMakeLists.txt | 1 + .../source/carla/ros2/dds/DDSMiddleware.h | 12 + .../carla/ros2/dds/DDSMiddlewareFactory.h | 6 +- .../carla/ros2/dds/fastdds/FastDDSTypeMap.h | 203 +++++- .../carla/ros2/publishers/PublisherImpl.h | 18 +- .../carla/ros2/subscribers/SubscriberImpl.h | 23 +- .../carla/ros2/types/FastDDSConversions.h | 623 ++++++++++++++++++ .../carla/ros2/types/msg/AckermannDrive.h | 21 + .../ros2/types/msg/AckermannDriveStamped.h | 20 + .../source/carla/ros2/types/msg/CameraInfo.h | 33 + .../ros2/types/msg/CarlaCollisionEvent.h | 22 + .../ros2/types/msg/CarlaEgoVehicleControl.h | 26 + .../carla/ros2/types/msg/CarlaLineInvasion.h | 21 + LibCarla/source/carla/ros2/types/msg/Clock.h | 18 + .../source/carla/ros2/types/msg/Float32.h | 17 + LibCarla/source/carla/ros2/types/msg/Header.h | 20 + LibCarla/source/carla/ros2/types/msg/Image.h | 27 + LibCarla/source/carla/ros2/types/msg/Imu.h | 27 + .../source/carla/ros2/types/msg/NavSatFix.h | 32 + .../carla/ros2/types/msg/NavSatStatus.h | 28 + .../source/carla/ros2/types/msg/Odometry.h | 24 + LibCarla/source/carla/ros2/types/msg/Point.h | 19 + .../source/carla/ros2/types/msg/Point32.h | 19 + .../source/carla/ros2/types/msg/PointCloud2.h | 29 + .../source/carla/ros2/types/msg/PointField.h | 31 + LibCarla/source/carla/ros2/types/msg/Pose.h | 20 + .../carla/ros2/types/msg/PoseWithCovariance.h | 20 + .../source/carla/ros2/types/msg/Quaternion.h | 20 + .../carla/ros2/types/msg/RegionOfInterest.h | 22 + LibCarla/source/carla/ros2/types/msg/String.h | 18 + .../source/carla/ros2/types/msg/TF2Error.h | 20 + .../source/carla/ros2/types/msg/TFMessage.h | 19 + LibCarla/source/carla/ros2/types/msg/Time.h | 19 + .../source/carla/ros2/types/msg/Transform.h | 20 + .../carla/ros2/types/msg/TransformStamped.h | 22 + LibCarla/source/carla/ros2/types/msg/Twist.h | 19 + .../ros2/types/msg/TwistWithCovariance.h | 20 + .../source/carla/ros2/types/msg/Vector3.h | 19 + .../test/server/test_dds_middleware.cpp | 490 ++++++++++++++ 39 files changed, 2049 insertions(+), 19 deletions(-) create mode 100644 LibCarla/source/carla/ros2/types/FastDDSConversions.h create mode 100644 LibCarla/source/carla/ros2/types/msg/AckermannDrive.h create mode 100644 LibCarla/source/carla/ros2/types/msg/AckermannDriveStamped.h create mode 100644 LibCarla/source/carla/ros2/types/msg/CameraInfo.h create mode 100644 LibCarla/source/carla/ros2/types/msg/CarlaCollisionEvent.h create mode 100644 LibCarla/source/carla/ros2/types/msg/CarlaEgoVehicleControl.h create mode 100644 LibCarla/source/carla/ros2/types/msg/CarlaLineInvasion.h create mode 100644 LibCarla/source/carla/ros2/types/msg/Clock.h create mode 100644 LibCarla/source/carla/ros2/types/msg/Float32.h create mode 100644 LibCarla/source/carla/ros2/types/msg/Header.h create mode 100644 LibCarla/source/carla/ros2/types/msg/Image.h create mode 100644 LibCarla/source/carla/ros2/types/msg/Imu.h create mode 100644 LibCarla/source/carla/ros2/types/msg/NavSatFix.h create mode 100644 LibCarla/source/carla/ros2/types/msg/NavSatStatus.h create mode 100644 LibCarla/source/carla/ros2/types/msg/Odometry.h create mode 100644 LibCarla/source/carla/ros2/types/msg/Point.h create mode 100644 LibCarla/source/carla/ros2/types/msg/Point32.h create mode 100644 LibCarla/source/carla/ros2/types/msg/PointCloud2.h create mode 100644 LibCarla/source/carla/ros2/types/msg/PointField.h create mode 100644 LibCarla/source/carla/ros2/types/msg/Pose.h create mode 100644 LibCarla/source/carla/ros2/types/msg/PoseWithCovariance.h create mode 100644 LibCarla/source/carla/ros2/types/msg/Quaternion.h create mode 100644 LibCarla/source/carla/ros2/types/msg/RegionOfInterest.h create mode 100644 LibCarla/source/carla/ros2/types/msg/String.h create mode 100644 LibCarla/source/carla/ros2/types/msg/TF2Error.h create mode 100644 LibCarla/source/carla/ros2/types/msg/TFMessage.h create mode 100644 LibCarla/source/carla/ros2/types/msg/Time.h create mode 100644 LibCarla/source/carla/ros2/types/msg/Transform.h create mode 100644 LibCarla/source/carla/ros2/types/msg/TransformStamped.h create mode 100644 LibCarla/source/carla/ros2/types/msg/Twist.h create mode 100644 LibCarla/source/carla/ros2/types/msg/TwistWithCovariance.h create mode 100644 LibCarla/source/carla/ros2/types/msg/Vector3.h create mode 100644 LibCarla/source/test/server/test_dds_middleware.cpp diff --git a/LibCarla/cmake/fast_dds/CMakeLists.txt b/LibCarla/cmake/fast_dds/CMakeLists.txt index 4ff74c21d57..f51bbc430f7 100644 --- a/LibCarla/cmake/fast_dds/CMakeLists.txt +++ b/LibCarla/cmake/fast_dds/CMakeLists.txt @@ -9,6 +9,7 @@ file(GLOB libcarla_carla_fastdds_headers "${libcarla_source_path}/carla/ros2/subscribers/*.h" "${libcarla_source_path}/carla/ros2/listeners/*.h" "${libcarla_source_path}/carla/ros2/types/*.h" + "${libcarla_source_path}/carla/ros2/types/msg/*.h" "${libcarla_source_path}/carla/ros2/dds/*.h" "${libcarla_source_path}/carla/ros2/dds/fastdds/*.h" ) diff --git a/LibCarla/source/carla/ros2/dds/DDSMiddleware.h b/LibCarla/source/carla/ros2/dds/DDSMiddleware.h index 35a4d89be47..20f8804c628 100644 --- a/LibCarla/source/carla/ros2/dds/DDSMiddleware.h +++ b/LibCarla/source/carla/ros2/dds/DDSMiddleware.h @@ -52,5 +52,17 @@ inline std::string GetAvailableMiddlewareString() { return result; } +/// Mangle a DDS type name into the ROS2-compatible format. +/// "sensor_msgs::msg::Image" becomes "sensor_msgs::msg::dds_::Image_". +/// A bare name like "Image" becomes "dds_::Image_". +inline std::string ToROS2DDSTypeName(const std::string& dds_type_name) { + auto pos = dds_type_name.rfind("::"); + if (pos == std::string::npos) { + return "dds_::" + dds_type_name + "_"; + } + return dds_type_name.substr(0, pos) + + "::dds_::" + dds_type_name.substr(pos + 2) + "_"; +} + } // namespace ros2 } // namespace carla diff --git a/LibCarla/source/carla/ros2/dds/DDSMiddlewareFactory.h b/LibCarla/source/carla/ros2/dds/DDSMiddlewareFactory.h index 5b7b2adcd9a..51cc8f81143 100644 --- a/LibCarla/source/carla/ros2/dds/DDSMiddlewareFactory.h +++ b/LibCarla/source/carla/ros2/dds/DDSMiddlewareFactory.h @@ -12,7 +12,7 @@ #include "carla/ros2/dds/IDDSSubscriberMiddleware.h" #include "carla/Logging.h" -#if defined(CARLA_ROS2_DDS_FASTDDS) +#if defined(CARLA_ROS2_DDS_FASTDDS) && !defined(CARLA_ROS2_DDS_TESTING) # include "carla/ros2/dds/fastdds/FastDDSPublisherMiddleware.h" # include "carla/ros2/dds/fastdds/FastDDSSubscriberMiddleware.h" #endif @@ -77,7 +77,7 @@ class DDSMiddlewareFactory { static std::unique_ptr CreatePublisher() { switch (GetActiveMiddleware()) { case DDSMiddleware::FastDDS: -#if defined(CARLA_ROS2_DDS_FASTDDS) +#if defined(CARLA_ROS2_DDS_FASTDDS) && !defined(CARLA_ROS2_DDS_TESTING) return std::unique_ptr( new FastDDSPublisherMiddleware()); #else @@ -95,7 +95,7 @@ class DDSMiddlewareFactory { static std::unique_ptr CreateSubscriber() { switch (GetActiveMiddleware()) { case DDSMiddleware::FastDDS: -#if defined(CARLA_ROS2_DDS_FASTDDS) +#if defined(CARLA_ROS2_DDS_FASTDDS) && !defined(CARLA_ROS2_DDS_TESTING) return std::unique_ptr( new FastDDSSubscriberMiddleware()); #else diff --git a/LibCarla/source/carla/ros2/dds/fastdds/FastDDSTypeMap.h b/LibCarla/source/carla/ros2/dds/fastdds/FastDDSTypeMap.h index 7a292f3a2b1..1a9ad1935a1 100644 --- a/LibCarla/source/carla/ros2/dds/fastdds/FastDDSTypeMap.h +++ b/LibCarla/source/carla/ros2/dds/fastdds/FastDDSTypeMap.h @@ -2,22 +2,42 @@ // This work is licensed under the terms of the MIT license. // For a copy, see . -// Identity-mapping version: msg_type IS the FastDDS type, so each -// specialization maps a type to itself. - #pragma once +#include "carla/ros2/types/FastDDSConversions.h" + // PubSubType headers for type registration +#include "carla/ros2/types/TimePubSubTypes.h" +#include "carla/ros2/types/HeaderPubSubTypes.h" +#include "carla/ros2/types/Vector3PubSubTypes.h" +#include "carla/ros2/types/QuaternionPubSubTypes.h" +#include "carla/ros2/types/PointPubSubTypes.h" +#include "carla/ros2/types/Point32PubSubTypes.h" +#include "carla/ros2/types/PosePubSubTypes.h" +#include "carla/ros2/types/PoseWithCovariancePubSubTypes.h" +#include "carla/ros2/types/TwistPubSubTypes.h" +#include "carla/ros2/types/TwistWithCovariancePubSubTypes.h" +#include "carla/ros2/types/TransformPubSubTypes.h" +#include "carla/ros2/types/TransformStampedPubSubTypes.h" +#include "carla/ros2/types/OdometryPubSubTypes.h" +#include "carla/ros2/types/RegionOfInterestPubSubTypes.h" +#include "carla/ros2/types/PointFieldPubSubTypes.h" +#include "carla/ros2/types/NavSatStatusPubSubTypes.h" #include "carla/ros2/types/NavSatFixPubSubTypes.h" +#include "carla/ros2/types/ClockPubSubTypes.h" +#include "carla/ros2/types/Float32PubSubTypes.h" +#include "carla/ros2/types/StringPubSubTypes.h" +#include "carla/ros2/types/ImuPubSubTypes.h" #include "carla/ros2/types/ImagePubSubTypes.h" #include "carla/ros2/types/CameraInfoPubSubTypes.h" -#include "carla/ros2/types/ImuPubSubTypes.h" #include "carla/ros2/types/PointCloud2PubSubTypes.h" -#include "carla/ros2/types/ClockPubSubTypes.h" #include "carla/ros2/types/TFMessagePubSubTypes.h" +#include "carla/ros2/types/TF2ErrorPubSubTypes.h" +#include "carla/ros2/types/AckermannDrivePubSubTypes.h" +#include "carla/ros2/types/AckermannDriveStampedPubSubTypes.h" #include "carla/ros2/types/CarlaCollisionEventPubSubTypes.h" #include "carla/ros2/types/CarlaEgoVehicleControlPubSubTypes.h" -#include "carla/ros2/types/AckermannDriveStampedPubSubTypes.h" +#include "carla/ros2/types/CarlaLineInvasionPubSubTypes.h" namespace carla { namespace ros2 { @@ -26,9 +46,11 @@ namespace ros2 { /// Primary template is intentionally undefined — only specializations are valid. template struct FastDDSTypeMap; -// --- Identity specializations --- -// In the current codebase, msg_type is already the FastDDS-generated type. -// Each specialization maps the type to itself and its corresponding PubSubType. +// ============================================================ +// Identity specializations (msg_type IS the FastDDS type) +// Used by current publishers/subscribers until they are migrated +// to POD message types in PR #2b. +// ============================================================ template<> struct FastDDSTypeMap { using fastdds_type = sensor_msgs::msg::NavSatFix; @@ -81,7 +103,7 @@ template<> struct FastDDSTypeMap { }; /// Identity conversion: copy src to dst when both types are the same. -/// In the next phase, we will replace these with real POD-to-FastDDS conversions. +/// Used by publishers/subscribers that have not yet migrated to POD types. template inline void to_fastdds(const T& src, T& dst) { dst = src; @@ -92,5 +114,166 @@ inline void from_fastdds(const T& src, T& dst) { dst = src; } +// ============================================================ +// Real specializations (POD msg types -> FastDDS types) +// These map backend-neutral POD structs to FastDDS-generated types. +// Conversion functions are in types/FastDDSConversions.h. +// ============================================================ + +template<> struct FastDDSTypeMap { + using fastdds_type = builtin_interfaces::msg::Time; + using fastdds_pubsub_type = builtin_interfaces::msg::TimePubSubType; +}; + +template<> struct FastDDSTypeMap { + using fastdds_type = std_msgs::msg::Header; + using fastdds_pubsub_type = std_msgs::msg::HeaderPubSubType; +}; + +template<> struct FastDDSTypeMap { + using fastdds_type = geometry_msgs::msg::Vector3; + using fastdds_pubsub_type = geometry_msgs::msg::Vector3PubSubType; +}; + +template<> struct FastDDSTypeMap { + using fastdds_type = geometry_msgs::msg::Quaternion; + using fastdds_pubsub_type = geometry_msgs::msg::QuaternionPubSubType; +}; + +template<> struct FastDDSTypeMap { + using fastdds_type = geometry_msgs::msg::Point; + using fastdds_pubsub_type = geometry_msgs::msg::PointPubSubType; +}; + +template<> struct FastDDSTypeMap { + using fastdds_type = geometry_msgs::msg::Point32; + using fastdds_pubsub_type = geometry_msgs::msg::Point32PubSubType; +}; + +template<> struct FastDDSTypeMap { + using fastdds_type = geometry_msgs::msg::Pose; + using fastdds_pubsub_type = geometry_msgs::msg::PosePubSubType; +}; + +template<> struct FastDDSTypeMap { + using fastdds_type = geometry_msgs::msg::PoseWithCovariance; + using fastdds_pubsub_type = geometry_msgs::msg::PoseWithCovariancePubSubType; +}; + +template<> struct FastDDSTypeMap { + using fastdds_type = geometry_msgs::msg::Twist; + using fastdds_pubsub_type = geometry_msgs::msg::TwistPubSubType; +}; + +template<> struct FastDDSTypeMap { + using fastdds_type = geometry_msgs::msg::TwistWithCovariance; + using fastdds_pubsub_type = geometry_msgs::msg::TwistWithCovariancePubSubType; +}; + +template<> struct FastDDSTypeMap { + using fastdds_type = geometry_msgs::msg::Transform; + using fastdds_pubsub_type = geometry_msgs::msg::TransformPubSubType; +}; + +template<> struct FastDDSTypeMap { + using fastdds_type = geometry_msgs::msg::TransformStamped; + using fastdds_pubsub_type = geometry_msgs::msg::TransformStampedPubSubType; +}; + +template<> struct FastDDSTypeMap { + using fastdds_type = nav_msgs::msg::Odometry; + using fastdds_pubsub_type = nav_msgs::msg::OdometryPubSubType; +}; + +template<> struct FastDDSTypeMap { + using fastdds_type = sensor_msgs::msg::RegionOfInterest; + using fastdds_pubsub_type = sensor_msgs::msg::RegionOfInterestPubSubType; +}; + +template<> struct FastDDSTypeMap { + using fastdds_type = sensor_msgs::msg::PointField; + using fastdds_pubsub_type = sensor_msgs::msg::PointFieldPubSubType; +}; + +template<> struct FastDDSTypeMap { + using fastdds_type = sensor_msgs::msg::NavSatStatus; + using fastdds_pubsub_type = sensor_msgs::msg::NavSatStatusPubSubType; +}; + +template<> struct FastDDSTypeMap { + using fastdds_type = sensor_msgs::msg::NavSatFix; + using fastdds_pubsub_type = sensor_msgs::msg::NavSatFixPubSubType; +}; + +template<> struct FastDDSTypeMap { + using fastdds_type = rosgraph::msg::Clock; + using fastdds_pubsub_type = rosgraph::msg::ClockPubSubType; +}; + +template<> struct FastDDSTypeMap { + using fastdds_type = std_msgs::msg::Float32; + using fastdds_pubsub_type = std_msgs::msg::Float32PubSubType; +}; + +template<> struct FastDDSTypeMap { + using fastdds_type = std_msgs::msg::String; + using fastdds_pubsub_type = std_msgs::msg::StringPubSubType; +}; + +template<> struct FastDDSTypeMap { + using fastdds_type = sensor_msgs::msg::Imu; + using fastdds_pubsub_type = sensor_msgs::msg::ImuPubSubType; +}; + +template<> struct FastDDSTypeMap { + using fastdds_type = sensor_msgs::msg::Image; + using fastdds_pubsub_type = sensor_msgs::msg::ImagePubSubType; +}; + +template<> struct FastDDSTypeMap { + using fastdds_type = sensor_msgs::msg::CameraInfo; + using fastdds_pubsub_type = sensor_msgs::msg::CameraInfoPubSubType; +}; + +template<> struct FastDDSTypeMap { + using fastdds_type = sensor_msgs::msg::PointCloud2; + using fastdds_pubsub_type = sensor_msgs::msg::PointCloud2PubSubType; +}; + +template<> struct FastDDSTypeMap { + using fastdds_type = tf2_msgs::msg::TFMessage; + using fastdds_pubsub_type = tf2_msgs::msg::TFMessagePubSubType; +}; + +template<> struct FastDDSTypeMap { + using fastdds_type = tf2_msgs::msg::TF2Error; + using fastdds_pubsub_type = tf2_msgs::msg::TF2ErrorPubSubType; +}; + +template<> struct FastDDSTypeMap { + using fastdds_type = ackermann_msgs::msg::AckermannDrive; + using fastdds_pubsub_type = ackermann_msgs::msg::AckermannDrivePubSubType; +}; + +template<> struct FastDDSTypeMap { + using fastdds_type = ackermann_msgs::msg::AckermannDriveStamped; + using fastdds_pubsub_type = ackermann_msgs::msg::AckermannDriveStampedPubSubType; +}; + +template<> struct FastDDSTypeMap { + using fastdds_type = carla_msgs::msg::CarlaCollisionEvent; + using fastdds_pubsub_type = carla_msgs::msg::CarlaCollisionEventPubSubType; +}; + +template<> struct FastDDSTypeMap { + using fastdds_type = carla_msgs::msg::CarlaEgoVehicleControl; + using fastdds_pubsub_type = carla_msgs::msg::CarlaEgoVehicleControlPubSubType; +}; + +template<> struct FastDDSTypeMap { + using fastdds_type = carla_msgs::msg::LaneInvasionEvent; + using fastdds_pubsub_type = carla_msgs::msg::LaneInvasionEventPubSubType; +}; + } // namespace ros2 } // namespace carla diff --git a/LibCarla/source/carla/ros2/publishers/PublisherImpl.h b/LibCarla/source/carla/ros2/publishers/PublisherImpl.h index 9d46419eab7..63495013725 100644 --- a/LibCarla/source/carla/ros2/publishers/PublisherImpl.h +++ b/LibCarla/source/carla/ros2/publishers/PublisherImpl.h @@ -19,11 +19,17 @@ namespace ros2 { using msg_type = typename T::msg_type; bool Init(std::string topic_name) { - _middleware = DDSMiddlewareFactory::CreatePublisher(); +#ifdef LIBCARLA_WITH_GTEST if (!_middleware) { - log_error("PublisherImpl: Failed to create middleware publisher"); - return false; +#endif + _middleware = DDSMiddlewareFactory::CreatePublisher(); + if (!_middleware) { + log_error("PublisherImpl: Failed to create middleware publisher"); + return false; + } +#ifdef LIBCARLA_WITH_GTEST } +#endif return _middleware->Init(topic_name); } @@ -53,6 +59,12 @@ namespace ros2 { return _middleware->Publish(&_message); } +#ifdef LIBCARLA_WITH_GTEST + void SetMiddlewareForTesting(std::unique_ptr middleware) { + _middleware = std::move(middleware); + } +#endif + private: std::unique_ptr _middleware; msg_type _message; diff --git a/LibCarla/source/carla/ros2/subscribers/SubscriberImpl.h b/LibCarla/source/carla/ros2/subscribers/SubscriberImpl.h index 5d1f21ea5c5..8938496d64e 100644 --- a/LibCarla/source/carla/ros2/subscribers/SubscriberImpl.h +++ b/LibCarla/source/carla/ros2/subscribers/SubscriberImpl.h @@ -20,11 +20,17 @@ namespace ros2 { using msg_type = typename S::msg_type; bool Init(std::string topic_name) { - _middleware = DDSMiddlewareFactory::CreateSubscriber(); +#ifdef LIBCARLA_WITH_GTEST if (!_middleware) { - log_error("SubscriberImpl: Failed to create middleware subscriber"); - return false; +#endif + _middleware = DDSMiddlewareFactory::CreateSubscriber(); + if (!_middleware) { + log_error("SubscriberImpl: Failed to create middleware subscriber"); + return false; + } +#ifdef LIBCARLA_WITH_GTEST } +#endif return _middleware->Init(topic_name, &_message, &_new_message); } @@ -49,6 +55,17 @@ namespace ros2 { bool HasNewMessage() { return _new_message; } +#ifdef LIBCARLA_WITH_GTEST + void SetMiddlewareForTesting(std::unique_ptr middleware) { + _middleware = std::move(middleware); + } + + void SimulateMessageReceiptForTesting(const msg_type& msg) { + _message = msg; + _new_message = true; + } +#endif + private: std::unique_ptr _middleware; msg_type _message; diff --git a/LibCarla/source/carla/ros2/types/FastDDSConversions.h b/LibCarla/source/carla/ros2/types/FastDDSConversions.h new file mode 100644 index 00000000000..9fdb7739ed7 --- /dev/null +++ b/LibCarla/source/carla/ros2/types/FastDDSConversions.h @@ -0,0 +1,623 @@ +// Copyright (c) 2025 Computer Vision Center (CVC) at the Universitat Autonoma de Barcelona (UAB). +// This work is licensed under the terms of the MIT license. +// For a copy, see . + +#pragma once + +// POD message types +#include "carla/ros2/types/msg/Time.h" +#include "carla/ros2/types/msg/Header.h" +#include "carla/ros2/types/msg/Vector3.h" +#include "carla/ros2/types/msg/Quaternion.h" +#include "carla/ros2/types/msg/Point.h" +#include "carla/ros2/types/msg/Point32.h" +#include "carla/ros2/types/msg/Pose.h" +#include "carla/ros2/types/msg/PoseWithCovariance.h" +#include "carla/ros2/types/msg/Twist.h" +#include "carla/ros2/types/msg/TwistWithCovariance.h" +#include "carla/ros2/types/msg/Transform.h" +#include "carla/ros2/types/msg/TransformStamped.h" +#include "carla/ros2/types/msg/Odometry.h" +#include "carla/ros2/types/msg/RegionOfInterest.h" +#include "carla/ros2/types/msg/PointField.h" +#include "carla/ros2/types/msg/NavSatStatus.h" +#include "carla/ros2/types/msg/NavSatFix.h" +#include "carla/ros2/types/msg/Clock.h" +#include "carla/ros2/types/msg/Float32.h" +#include "carla/ros2/types/msg/String.h" +#include "carla/ros2/types/msg/Imu.h" +#include "carla/ros2/types/msg/Image.h" +#include "carla/ros2/types/msg/CameraInfo.h" +#include "carla/ros2/types/msg/PointCloud2.h" +#include "carla/ros2/types/msg/TFMessage.h" +#include "carla/ros2/types/msg/TF2Error.h" +#include "carla/ros2/types/msg/AckermannDrive.h" +#include "carla/ros2/types/msg/AckermannDriveStamped.h" +#include "carla/ros2/types/msg/CarlaCollisionEvent.h" +#include "carla/ros2/types/msg/CarlaEgoVehicleControl.h" +#include "carla/ros2/types/msg/CarlaLineInvasion.h" + +// FastDDS-generated types (current flat layout) +#include "carla/ros2/types/Time.h" +#include "carla/ros2/types/Header.h" +#include "carla/ros2/types/Vector3.h" +#include "carla/ros2/types/Quaternion.h" +#include "carla/ros2/types/Point.h" +#include "carla/ros2/types/Point32.h" +#include "carla/ros2/types/Pose.h" +#include "carla/ros2/types/PoseWithCovariance.h" +#include "carla/ros2/types/Twist.h" +#include "carla/ros2/types/TwistWithCovariance.h" +#include "carla/ros2/types/Transform.h" +#include "carla/ros2/types/TransformStamped.h" +#include "carla/ros2/types/Odometry.h" +#include "carla/ros2/types/RegionOfInterest.h" +#include "carla/ros2/types/PointField.h" +#include "carla/ros2/types/NavSatStatus.h" +#include "carla/ros2/types/NavSatFix.h" +#include "carla/ros2/types/Clock.h" +#include "carla/ros2/types/Float32.h" +#include "carla/ros2/types/String.h" +#include "carla/ros2/types/Imu.h" +#include "carla/ros2/types/Image.h" +#include "carla/ros2/types/CameraInfo.h" +#include "carla/ros2/types/PointCloud2.h" +#include "carla/ros2/types/TFMessage.h" +#include "carla/ros2/types/TF2Error.h" +#include "carla/ros2/types/AckermannDrive.h" +#include "carla/ros2/types/AckermannDriveStamped.h" +#include "carla/ros2/types/CarlaCollisionEvent.h" +#include "carla/ros2/types/CarlaEgoVehicleControl.h" +#include "carla/ros2/types/CarlaLineInvasion.h" + +#include + +namespace carla { +namespace ros2 { + +// ============================================================ +// Conversion functions: POD msg types <-> FastDDS-generated types +// Order matters — composites call primitives. +// ============================================================ + +// --- Primitives --- + +inline void to_fastdds(const msg::Time& src, builtin_interfaces::msg::Time& dst) { + dst.sec(src.sec); + dst.nanosec(src.nanosec); +} + +inline void from_fastdds(const builtin_interfaces::msg::Time& src, msg::Time& dst) { + dst.sec = src.sec(); + dst.nanosec = src.nanosec(); +} + +inline void to_fastdds(const msg::Header& src, std_msgs::msg::Header& dst) { + to_fastdds(src.stamp, dst.stamp()); + dst.frame_id(src.frame_id); +} + +inline void from_fastdds(const std_msgs::msg::Header& src, msg::Header& dst) { + from_fastdds(src.stamp(), dst.stamp); + dst.frame_id = src.frame_id(); +} + +inline void to_fastdds(const msg::Vector3& src, geometry_msgs::msg::Vector3& dst) { + dst.x(src.x); + dst.y(src.y); + dst.z(src.z); +} + +inline void from_fastdds(const geometry_msgs::msg::Vector3& src, msg::Vector3& dst) { + dst.x = src.x(); + dst.y = src.y(); + dst.z = src.z(); +} + +inline void to_fastdds( + const msg::Quaternion& src, + geometry_msgs::msg::Quaternion& dst) { + dst.x(src.x); + dst.y(src.y); + dst.z(src.z); + dst.w(src.w); +} + +inline void from_fastdds( + const geometry_msgs::msg::Quaternion& src, + msg::Quaternion& dst) { + dst.x = src.x(); + dst.y = src.y(); + dst.z = src.z(); + dst.w = src.w(); +} + +inline void to_fastdds(const msg::Point& src, geometry_msgs::msg::Point& dst) { + dst.x(src.x); + dst.y(src.y); + dst.z(src.z); +} + +inline void from_fastdds(const geometry_msgs::msg::Point& src, msg::Point& dst) { + dst.x = src.x(); + dst.y = src.y(); + dst.z = src.z(); +} + +inline void to_fastdds(const msg::Point32& src, geometry_msgs::msg::Point32& dst) { + dst.x(src.x); + dst.y(src.y); + dst.z(src.z); +} + +inline void from_fastdds(const geometry_msgs::msg::Point32& src, msg::Point32& dst) { + dst.x = src.x(); + dst.y = src.y(); + dst.z = src.z(); +} + +// --- Geometry composites --- + +inline void to_fastdds(const msg::Pose& src, geometry_msgs::msg::Pose& dst) { + to_fastdds(src.position, dst.position()); + to_fastdds(src.orientation, dst.orientation()); +} + +inline void from_fastdds(const geometry_msgs::msg::Pose& src, msg::Pose& dst) { + from_fastdds(src.position(), dst.position); + from_fastdds(src.orientation(), dst.orientation); +} + +inline void to_fastdds( + const msg::PoseWithCovariance& src, + geometry_msgs::msg::PoseWithCovariance& dst) { + to_fastdds(src.pose, dst.pose()); + dst.covariance(src.covariance); +} + +inline void from_fastdds( + const geometry_msgs::msg::PoseWithCovariance& src, + msg::PoseWithCovariance& dst) { + from_fastdds(src.pose(), dst.pose); + dst.covariance = src.covariance(); +} + +inline void to_fastdds(const msg::Twist& src, geometry_msgs::msg::Twist& dst) { + to_fastdds(src.linear, dst.linear()); + to_fastdds(src.angular, dst.angular()); +} + +inline void from_fastdds(const geometry_msgs::msg::Twist& src, msg::Twist& dst) { + from_fastdds(src.linear(), dst.linear); + from_fastdds(src.angular(), dst.angular); +} + +inline void to_fastdds( + const msg::TwistWithCovariance& src, + geometry_msgs::msg::TwistWithCovariance& dst) { + to_fastdds(src.twist, dst.twist()); + dst.covariance(src.covariance); +} + +inline void from_fastdds( + const geometry_msgs::msg::TwistWithCovariance& src, + msg::TwistWithCovariance& dst) { + from_fastdds(src.twist(), dst.twist); + dst.covariance = src.covariance(); +} + +inline void to_fastdds( + const msg::Transform& src, + geometry_msgs::msg::Transform& dst) { + to_fastdds(src.translation, dst.translation()); + to_fastdds(src.rotation, dst.rotation()); +} + +inline void from_fastdds( + const geometry_msgs::msg::Transform& src, + msg::Transform& dst) { + from_fastdds(src.translation(), dst.translation); + from_fastdds(src.rotation(), dst.rotation); +} + +inline void to_fastdds( + const msg::TransformStamped& src, + geometry_msgs::msg::TransformStamped& dst) { + to_fastdds(src.header, dst.header()); + dst.child_frame_id(src.child_frame_id); + to_fastdds(src.transform, dst.transform()); +} + +inline void from_fastdds( + const geometry_msgs::msg::TransformStamped& src, + msg::TransformStamped& dst) { + from_fastdds(src.header(), dst.header); + dst.child_frame_id = src.child_frame_id(); + from_fastdds(src.transform(), dst.transform); +} + +// --- Sensor primitives --- + +inline void to_fastdds( + const msg::RegionOfInterest& src, + sensor_msgs::msg::RegionOfInterest& dst) { + dst.x_offset(src.x_offset); + dst.y_offset(src.y_offset); + dst.height(src.height); + dst.width(src.width); + dst.do_rectify(src.do_rectify); +} + +inline void from_fastdds( + const sensor_msgs::msg::RegionOfInterest& src, + msg::RegionOfInterest& dst) { + dst.x_offset = src.x_offset(); + dst.y_offset = src.y_offset(); + dst.height = src.height(); + dst.width = src.width(); + dst.do_rectify = src.do_rectify(); +} + +inline void to_fastdds( + const msg::PointField& src, + sensor_msgs::msg::PointField& dst) { + dst.name(src.name); + dst.offset(src.offset); + dst.datatype(src.datatype); + dst.count(src.count); +} + +inline void from_fastdds( + const sensor_msgs::msg::PointField& src, + msg::PointField& dst) { + dst.name = src.name(); + dst.offset = src.offset(); + dst.datatype = src.datatype(); + dst.count = src.count(); +} + +inline void to_fastdds( + const msg::NavSatStatus& src, + sensor_msgs::msg::NavSatStatus& dst) { + dst.status(src.status); + dst.service(src.service); +} + +inline void from_fastdds( + const sensor_msgs::msg::NavSatStatus& src, + msg::NavSatStatus& dst) { + dst.status = src.status(); + dst.service = src.service(); +} + +// --- Simple messages --- + +inline void to_fastdds(const msg::Clock& src, rosgraph::msg::Clock& dst) { + to_fastdds(src.clock, dst.clock()); +} + +inline void from_fastdds(const rosgraph::msg::Clock& src, msg::Clock& dst) { + from_fastdds(src.clock(), dst.clock); +} + +inline void to_fastdds(const msg::Float32& src, std_msgs::msg::Float32& dst) { + dst.data(src.data); +} + +inline void from_fastdds(const std_msgs::msg::Float32& src, msg::Float32& dst) { + dst.data = src.data(); +} + +inline void to_fastdds(const msg::String& src, std_msgs::msg::String& dst) { + dst.data(src.data); +} + +inline void from_fastdds(const std_msgs::msg::String& src, msg::String& dst) { + dst.data = src.data(); +} + +// --- Complex sensor messages --- + +inline void to_fastdds(const msg::Imu& src, sensor_msgs::msg::Imu& dst) { + to_fastdds(src.header, dst.header()); + to_fastdds(src.orientation, dst.orientation()); + dst.orientation_covariance(src.orientation_covariance); + to_fastdds(src.angular_velocity, dst.angular_velocity()); + dst.angular_velocity_covariance(src.angular_velocity_covariance); + to_fastdds(src.linear_acceleration, dst.linear_acceleration()); + dst.linear_acceleration_covariance(src.linear_acceleration_covariance); +} + +inline void from_fastdds(const sensor_msgs::msg::Imu& src, msg::Imu& dst) { + from_fastdds(src.header(), dst.header); + from_fastdds(src.orientation(), dst.orientation); + dst.orientation_covariance = src.orientation_covariance(); + from_fastdds(src.angular_velocity(), dst.angular_velocity); + dst.angular_velocity_covariance = src.angular_velocity_covariance(); + from_fastdds(src.linear_acceleration(), dst.linear_acceleration); + dst.linear_acceleration_covariance = src.linear_acceleration_covariance(); +} + +inline void to_fastdds( + const msg::NavSatFix& src, + sensor_msgs::msg::NavSatFix& dst) { + to_fastdds(src.header, dst.header()); + to_fastdds(src.status, dst.status()); + dst.latitude(src.latitude); + dst.longitude(src.longitude); + dst.altitude(src.altitude); + dst.position_covariance(src.position_covariance); + dst.position_covariance_type(src.position_covariance_type); +} + +inline void from_fastdds( + const sensor_msgs::msg::NavSatFix& src, + msg::NavSatFix& dst) { + from_fastdds(src.header(), dst.header); + from_fastdds(src.status(), dst.status); + dst.latitude = src.latitude(); + dst.longitude = src.longitude(); + dst.altitude = src.altitude(); + dst.position_covariance = src.position_covariance(); + dst.position_covariance_type = src.position_covariance_type(); +} + +// Image: const-ref overload (copies data vector) +inline void to_fastdds(const msg::Image& src, sensor_msgs::msg::Image& dst) { + to_fastdds(src.header, dst.header()); + dst.height(src.height); + dst.width(src.width); + dst.encoding(src.encoding); + dst.is_bigendian(src.is_bigendian); + dst.step(src.step); + dst.data(src.data); +} + +// Image: non-const overload (moves data vector) +inline void to_fastdds(msg::Image& src, sensor_msgs::msg::Image& dst) { + to_fastdds(src.header, dst.header()); + dst.height(src.height); + dst.width(src.width); + dst.encoding(std::move(src.encoding)); + dst.is_bigendian(src.is_bigendian); + dst.step(src.step); + dst.data(std::move(src.data)); +} + +inline void from_fastdds(const sensor_msgs::msg::Image& src, msg::Image& dst) { + from_fastdds(src.header(), dst.header); + dst.height = src.height(); + dst.width = src.width(); + dst.encoding = src.encoding(); + dst.is_bigendian = src.is_bigendian(); + dst.step = src.step(); + dst.data = src.data(); +} + +inline void to_fastdds( + const msg::CameraInfo& src, + sensor_msgs::msg::CameraInfo& dst) { + to_fastdds(src.header, dst.header()); + dst.height(src.height); + dst.width(src.width); + dst.distortion_model(src.distortion_model); + dst.D(src.d); + dst.k(src.k); + dst.r(src.r); + dst.p(src.p); + dst.binning_x(src.binning_x); + dst.binning_y(src.binning_y); + to_fastdds(src.roi, dst.roi()); +} + +inline void from_fastdds( + const sensor_msgs::msg::CameraInfo& src, + msg::CameraInfo& dst) { + from_fastdds(src.header(), dst.header); + dst.height = src.height(); + dst.width = src.width(); + dst.distortion_model = src.distortion_model(); + dst.d = src.D(); + dst.k = src.k(); + dst.r = src.r(); + dst.p = src.p(); + dst.binning_x = src.binning_x(); + dst.binning_y = src.binning_y(); + from_fastdds(src.roi(), dst.roi); +} + +// PointCloud2: const-ref overload (copies data vector) +inline void to_fastdds( + const msg::PointCloud2& src, + sensor_msgs::msg::PointCloud2& dst) { + to_fastdds(src.header, dst.header()); + dst.height(src.height); + dst.width(src.width); + std::vector fields(src.fields.size()); + for (size_t i = 0; i < src.fields.size(); ++i) { + to_fastdds(src.fields[i], fields[i]); + } + dst.fields(std::move(fields)); + dst.is_bigendian(src.is_bigendian); + dst.point_step(src.point_step); + dst.row_step(src.row_step); + dst.data(src.data); + dst.is_dense(src.is_dense); +} + +// PointCloud2: non-const overload (moves data vector) +inline void to_fastdds( + msg::PointCloud2& src, + sensor_msgs::msg::PointCloud2& dst) { + to_fastdds(src.header, dst.header()); + dst.height(src.height); + dst.width(src.width); + std::vector fields(src.fields.size()); + for (size_t i = 0; i < src.fields.size(); ++i) { + to_fastdds(src.fields[i], fields[i]); + } + dst.fields(std::move(fields)); + dst.is_bigendian(src.is_bigendian); + dst.point_step(src.point_step); + dst.row_step(src.row_step); + dst.data(std::move(src.data)); + dst.is_dense(src.is_dense); +} + +inline void from_fastdds( + const sensor_msgs::msg::PointCloud2& src, + msg::PointCloud2& dst) { + from_fastdds(src.header(), dst.header); + dst.height = src.height(); + dst.width = src.width(); + dst.fields.resize(src.fields().size()); + for (size_t i = 0; i < src.fields().size(); ++i) { + from_fastdds(src.fields()[i], dst.fields[i]); + } + dst.is_bigendian = src.is_bigendian(); + dst.point_step = src.point_step(); + dst.row_step = src.row_step(); + dst.data = src.data(); + dst.is_dense = src.is_dense(); +} + +// --- tf2 messages --- + +inline void to_fastdds(const msg::TFMessage& src, tf2_msgs::msg::TFMessage& dst) { + std::vector transforms(src.transforms.size()); + for (size_t i = 0; i < src.transforms.size(); ++i) { + to_fastdds(src.transforms[i], transforms[i]); + } + dst.transforms(std::move(transforms)); +} + +inline void from_fastdds(const tf2_msgs::msg::TFMessage& src, msg::TFMessage& dst) { + dst.transforms.resize(src.transforms().size()); + for (size_t i = 0; i < src.transforms().size(); ++i) { + from_fastdds(src.transforms()[i], dst.transforms[i]); + } +} + +inline void to_fastdds(const msg::TF2Error& src, tf2_msgs::msg::TF2Error& dst) { + dst.error(src.error); + dst.error_string(src.error_string); +} + +inline void from_fastdds(const tf2_msgs::msg::TF2Error& src, msg::TF2Error& dst) { + dst.error = src.error(); + dst.error_string = src.error_string(); +} + +// --- Navigation --- + +inline void to_fastdds(const msg::Odometry& src, nav_msgs::msg::Odometry& dst) { + to_fastdds(src.header, dst.header()); + dst.child_frame_id(src.child_frame_id); + to_fastdds(src.pose, dst.pose()); + to_fastdds(src.twist, dst.twist()); +} + +inline void from_fastdds(const nav_msgs::msg::Odometry& src, msg::Odometry& dst) { + from_fastdds(src.header(), dst.header); + dst.child_frame_id = src.child_frame_id(); + from_fastdds(src.pose(), dst.pose); + from_fastdds(src.twist(), dst.twist); +} + +// --- Ackermann --- + +inline void to_fastdds( + const msg::AckermannDrive& src, + ackermann_msgs::msg::AckermannDrive& dst) { + dst.steering_angle(src.steering_angle); + dst.steering_angle_velocity(src.steering_angle_velocity); + dst.speed(src.speed); + dst.acceleration(src.acceleration); + dst.jerk(src.jerk); +} + +inline void from_fastdds( + const ackermann_msgs::msg::AckermannDrive& src, + msg::AckermannDrive& dst) { + dst.steering_angle = src.steering_angle(); + dst.steering_angle_velocity = src.steering_angle_velocity(); + dst.speed = src.speed(); + dst.acceleration = src.acceleration(); + dst.jerk = src.jerk(); +} + +inline void to_fastdds( + const msg::AckermannDriveStamped& src, + ackermann_msgs::msg::AckermannDriveStamped& dst) { + to_fastdds(src.header, dst.header()); + to_fastdds(src.drive, dst.drive()); +} + +inline void from_fastdds( + const ackermann_msgs::msg::AckermannDriveStamped& src, + msg::AckermannDriveStamped& dst) { + from_fastdds(src.header(), dst.header); + from_fastdds(src.drive(), dst.drive); +} + +// --- CARLA custom messages --- + +inline void to_fastdds( + const msg::CarlaCollisionEvent& src, + carla_msgs::msg::CarlaCollisionEvent& dst) { + to_fastdds(src.header, dst.header()); + dst.other_actor_id(src.other_actor_id); + to_fastdds(src.normal_impulse, dst.normal_impulse()); +} + +inline void from_fastdds( + const carla_msgs::msg::CarlaCollisionEvent& src, + msg::CarlaCollisionEvent& dst) { + from_fastdds(src.header(), dst.header); + dst.other_actor_id = src.other_actor_id(); + from_fastdds(src.normal_impulse(), dst.normal_impulse); +} + +inline void to_fastdds( + const msg::CarlaEgoVehicleControl& src, + carla_msgs::msg::CarlaEgoVehicleControl& dst) { + to_fastdds(src.header, dst.header()); + dst.throttle(src.throttle); + dst.steer(src.steer); + dst.brake(src.brake); + dst.hand_brake(src.hand_brake); + dst.reverse(src.reverse); + dst.gear(src.gear); + dst.manual_gear_shift(src.manual_gear_shift); +} + +inline void from_fastdds( + const carla_msgs::msg::CarlaEgoVehicleControl& src, + msg::CarlaEgoVehicleControl& dst) { + from_fastdds(src.header(), dst.header); + dst.throttle = src.throttle(); + dst.steer = src.steer(); + dst.brake = src.brake(); + dst.hand_brake = src.hand_brake(); + dst.reverse = src.reverse(); + dst.gear = src.gear(); + dst.manual_gear_shift = src.manual_gear_shift(); +} + +// POD is CarlaLineInvasion, FastDDS class is LaneInvasionEvent +inline void to_fastdds( + const msg::CarlaLineInvasion& src, + carla_msgs::msg::LaneInvasionEvent& dst) { + to_fastdds(src.header, dst.header()); + dst.crossed_lane_markings(src.crossed_lane_markings); +} + +inline void from_fastdds( + const carla_msgs::msg::LaneInvasionEvent& src, + msg::CarlaLineInvasion& dst) { + from_fastdds(src.header(), dst.header); + dst.crossed_lane_markings = src.crossed_lane_markings(); +} + +} // namespace ros2 +} // namespace carla diff --git a/LibCarla/source/carla/ros2/types/msg/AckermannDrive.h b/LibCarla/source/carla/ros2/types/msg/AckermannDrive.h new file mode 100644 index 00000000000..259021b6f96 --- /dev/null +++ b/LibCarla/source/carla/ros2/types/msg/AckermannDrive.h @@ -0,0 +1,21 @@ +// Copyright (c) 2025 Computer Vision Center (CVC) at the Universitat Autonoma de Barcelona (UAB). +// This work is licensed under the terms of the MIT license. +// For a copy, see . + +#pragma once + +namespace carla { +namespace ros2 { +namespace msg { + +struct AckermannDrive { + float steering_angle = 0.0f; + float steering_angle_velocity = 0.0f; + float speed = 0.0f; + float acceleration = 0.0f; + float jerk = 0.0f; +}; + +} // namespace msg +} // namespace ros2 +} // namespace carla diff --git a/LibCarla/source/carla/ros2/types/msg/AckermannDriveStamped.h b/LibCarla/source/carla/ros2/types/msg/AckermannDriveStamped.h new file mode 100644 index 00000000000..003fe6823c1 --- /dev/null +++ b/LibCarla/source/carla/ros2/types/msg/AckermannDriveStamped.h @@ -0,0 +1,20 @@ +// Copyright (c) 2025 Computer Vision Center (CVC) at the Universitat Autonoma de Barcelona (UAB). +// This work is licensed under the terms of the MIT license. +// For a copy, see . + +#pragma once +#include "carla/ros2/types/msg/Header.h" +#include "carla/ros2/types/msg/AckermannDrive.h" + +namespace carla { +namespace ros2 { +namespace msg { + +struct AckermannDriveStamped { + Header header; + AckermannDrive drive; +}; + +} // namespace msg +} // namespace ros2 +} // namespace carla diff --git a/LibCarla/source/carla/ros2/types/msg/CameraInfo.h b/LibCarla/source/carla/ros2/types/msg/CameraInfo.h new file mode 100644 index 00000000000..1bc8fbf3a73 --- /dev/null +++ b/LibCarla/source/carla/ros2/types/msg/CameraInfo.h @@ -0,0 +1,33 @@ +// Copyright (c) 2025 Computer Vision Center (CVC) at the Universitat Autonoma de Barcelona (UAB). +// This work is licensed under the terms of the MIT license. +// For a copy, see . + +#pragma once +#include +#include +#include +#include +#include "carla/ros2/types/msg/Header.h" +#include "carla/ros2/types/msg/RegionOfInterest.h" + +namespace carla { +namespace ros2 { +namespace msg { + +struct CameraInfo { + Header header; + uint32_t height = 0; + uint32_t width = 0; + std::string distortion_model; + std::vector d; + std::array k = {}; + std::array r = {}; + std::array p = {}; + uint32_t binning_x = 0; + uint32_t binning_y = 0; + RegionOfInterest roi; +}; + +} // namespace msg +} // namespace ros2 +} // namespace carla diff --git a/LibCarla/source/carla/ros2/types/msg/CarlaCollisionEvent.h b/LibCarla/source/carla/ros2/types/msg/CarlaCollisionEvent.h new file mode 100644 index 00000000000..791cc9f90b9 --- /dev/null +++ b/LibCarla/source/carla/ros2/types/msg/CarlaCollisionEvent.h @@ -0,0 +1,22 @@ +// Copyright (c) 2025 Computer Vision Center (CVC) at the Universitat Autonoma de Barcelona (UAB). +// This work is licensed under the terms of the MIT license. +// For a copy, see . + +#pragma once +#include +#include "carla/ros2/types/msg/Header.h" +#include "carla/ros2/types/msg/Vector3.h" + +namespace carla { +namespace ros2 { +namespace msg { + +struct CarlaCollisionEvent { + Header header; + uint32_t other_actor_id = 0; + Vector3 normal_impulse; +}; + +} // namespace msg +} // namespace ros2 +} // namespace carla diff --git a/LibCarla/source/carla/ros2/types/msg/CarlaEgoVehicleControl.h b/LibCarla/source/carla/ros2/types/msg/CarlaEgoVehicleControl.h new file mode 100644 index 00000000000..9c7b90c4b96 --- /dev/null +++ b/LibCarla/source/carla/ros2/types/msg/CarlaEgoVehicleControl.h @@ -0,0 +1,26 @@ +// Copyright (c) 2025 Computer Vision Center (CVC) at the Universitat Autonoma de Barcelona (UAB). +// This work is licensed under the terms of the MIT license. +// For a copy, see . + +#pragma once +#include +#include "carla/ros2/types/msg/Header.h" + +namespace carla { +namespace ros2 { +namespace msg { + +struct CarlaEgoVehicleControl { + Header header; + float throttle = 0.0f; + float steer = 0.0f; + float brake = 0.0f; + bool hand_brake = false; + bool reverse = false; + int32_t gear = 0; + bool manual_gear_shift = false; +}; + +} // namespace msg +} // namespace ros2 +} // namespace carla diff --git a/LibCarla/source/carla/ros2/types/msg/CarlaLineInvasion.h b/LibCarla/source/carla/ros2/types/msg/CarlaLineInvasion.h new file mode 100644 index 00000000000..758ace94505 --- /dev/null +++ b/LibCarla/source/carla/ros2/types/msg/CarlaLineInvasion.h @@ -0,0 +1,21 @@ +// Copyright (c) 2025 Computer Vision Center (CVC) at the Universitat Autonoma de Barcelona (UAB). +// This work is licensed under the terms of the MIT license. +// For a copy, see . + +#pragma once +#include +#include +#include "carla/ros2/types/msg/Header.h" + +namespace carla { +namespace ros2 { +namespace msg { + +struct CarlaLineInvasion { + Header header; + std::vector crossed_lane_markings; +}; + +} // namespace msg +} // namespace ros2 +} // namespace carla diff --git a/LibCarla/source/carla/ros2/types/msg/Clock.h b/LibCarla/source/carla/ros2/types/msg/Clock.h new file mode 100644 index 00000000000..c3aeb22ef23 --- /dev/null +++ b/LibCarla/source/carla/ros2/types/msg/Clock.h @@ -0,0 +1,18 @@ +// Copyright (c) 2025 Computer Vision Center (CVC) at the Universitat Autonoma de Barcelona (UAB). +// This work is licensed under the terms of the MIT license. +// For a copy, see . + +#pragma once +#include "carla/ros2/types/msg/Time.h" + +namespace carla { +namespace ros2 { +namespace msg { + +struct Clock { + Time clock; +}; + +} // namespace msg +} // namespace ros2 +} // namespace carla diff --git a/LibCarla/source/carla/ros2/types/msg/Float32.h b/LibCarla/source/carla/ros2/types/msg/Float32.h new file mode 100644 index 00000000000..6452f152256 --- /dev/null +++ b/LibCarla/source/carla/ros2/types/msg/Float32.h @@ -0,0 +1,17 @@ +// Copyright (c) 2025 Computer Vision Center (CVC) at the Universitat Autonoma de Barcelona (UAB). +// This work is licensed under the terms of the MIT license. +// For a copy, see . + +#pragma once + +namespace carla { +namespace ros2 { +namespace msg { + +struct Float32 { + float data = 0.0f; +}; + +} // namespace msg +} // namespace ros2 +} // namespace carla diff --git a/LibCarla/source/carla/ros2/types/msg/Header.h b/LibCarla/source/carla/ros2/types/msg/Header.h new file mode 100644 index 00000000000..7def89c2f96 --- /dev/null +++ b/LibCarla/source/carla/ros2/types/msg/Header.h @@ -0,0 +1,20 @@ +// Copyright (c) 2025 Computer Vision Center (CVC) at the Universitat Autonoma de Barcelona (UAB). +// This work is licensed under the terms of the MIT license. +// For a copy, see . + +#pragma once +#include +#include "carla/ros2/types/msg/Time.h" + +namespace carla { +namespace ros2 { +namespace msg { + +struct Header { + Time stamp; + std::string frame_id; +}; + +} // namespace msg +} // namespace ros2 +} // namespace carla diff --git a/LibCarla/source/carla/ros2/types/msg/Image.h b/LibCarla/source/carla/ros2/types/msg/Image.h new file mode 100644 index 00000000000..6f106c1735e --- /dev/null +++ b/LibCarla/source/carla/ros2/types/msg/Image.h @@ -0,0 +1,27 @@ +// Copyright (c) 2025 Computer Vision Center (CVC) at the Universitat Autonoma de Barcelona (UAB). +// This work is licensed under the terms of the MIT license. +// For a copy, see . + +#pragma once +#include +#include +#include +#include "carla/ros2/types/msg/Header.h" + +namespace carla { +namespace ros2 { +namespace msg { + +struct Image { + Header header; + uint32_t height = 0; + uint32_t width = 0; + std::string encoding; + uint8_t is_bigendian = 0; + uint32_t step = 0; + std::vector data; +}; + +} // namespace msg +} // namespace ros2 +} // namespace carla diff --git a/LibCarla/source/carla/ros2/types/msg/Imu.h b/LibCarla/source/carla/ros2/types/msg/Imu.h new file mode 100644 index 00000000000..357c4e8fe07 --- /dev/null +++ b/LibCarla/source/carla/ros2/types/msg/Imu.h @@ -0,0 +1,27 @@ +// Copyright (c) 2025 Computer Vision Center (CVC) at the Universitat Autonoma de Barcelona (UAB). +// This work is licensed under the terms of the MIT license. +// For a copy, see . + +#pragma once +#include +#include "carla/ros2/types/msg/Header.h" +#include "carla/ros2/types/msg/Quaternion.h" +#include "carla/ros2/types/msg/Vector3.h" + +namespace carla { +namespace ros2 { +namespace msg { + +struct Imu { + Header header; + Quaternion orientation; + std::array orientation_covariance = {}; + Vector3 angular_velocity; + std::array angular_velocity_covariance = {}; + Vector3 linear_acceleration; + std::array linear_acceleration_covariance = {}; +}; + +} // namespace msg +} // namespace ros2 +} // namespace carla diff --git a/LibCarla/source/carla/ros2/types/msg/NavSatFix.h b/LibCarla/source/carla/ros2/types/msg/NavSatFix.h new file mode 100644 index 00000000000..842b777faef --- /dev/null +++ b/LibCarla/source/carla/ros2/types/msg/NavSatFix.h @@ -0,0 +1,32 @@ +// Copyright (c) 2025 Computer Vision Center (CVC) at the Universitat Autonoma de Barcelona (UAB). +// This work is licensed under the terms of the MIT license. +// For a copy, see . + +#pragma once +#include +#include +#include "carla/ros2/types/msg/Header.h" +#include "carla/ros2/types/msg/NavSatStatus.h" + +namespace carla { +namespace ros2 { +namespace msg { + +struct NavSatFix { + static const uint8_t COVARIANCE_TYPE_UNKNOWN = 0; + static const uint8_t COVARIANCE_TYPE_APPROXIMATED = 1; + static const uint8_t COVARIANCE_TYPE_DIAGONAL_KNOWN = 2; + static const uint8_t COVARIANCE_TYPE_KNOWN = 3; + + Header header; + NavSatStatus status; + double latitude = 0.0; + double longitude = 0.0; + double altitude = 0.0; + std::array position_covariance = {}; + uint8_t position_covariance_type = 0; +}; + +} // namespace msg +} // namespace ros2 +} // namespace carla diff --git a/LibCarla/source/carla/ros2/types/msg/NavSatStatus.h b/LibCarla/source/carla/ros2/types/msg/NavSatStatus.h new file mode 100644 index 00000000000..2470b2e9968 --- /dev/null +++ b/LibCarla/source/carla/ros2/types/msg/NavSatStatus.h @@ -0,0 +1,28 @@ +// Copyright (c) 2025 Computer Vision Center (CVC) at the Universitat Autonoma de Barcelona (UAB). +// This work is licensed under the terms of the MIT license. +// For a copy, see . + +#pragma once +#include + +namespace carla { +namespace ros2 { +namespace msg { + +struct NavSatStatus { + static const uint8_t STATUS_NO_FIX = 255; + static const uint8_t STATUS_FIX = 0; + static const uint8_t STATUS_SBAS_FIX = 1; + static const uint8_t STATUS_GBAS_FIX = 2; + static const uint16_t SERVICE_GPS = 1; + static const uint16_t SERVICE_GLONASS = 2; + static const uint16_t SERVICE_COMPASS = 4; + static const uint16_t SERVICE_GALILEO = 8; + + uint8_t status = 0; + uint16_t service = 0; +}; + +} // namespace msg +} // namespace ros2 +} // namespace carla diff --git a/LibCarla/source/carla/ros2/types/msg/Odometry.h b/LibCarla/source/carla/ros2/types/msg/Odometry.h new file mode 100644 index 00000000000..4d4e2d0a3e8 --- /dev/null +++ b/LibCarla/source/carla/ros2/types/msg/Odometry.h @@ -0,0 +1,24 @@ +// Copyright (c) 2025 Computer Vision Center (CVC) at the Universitat Autonoma de Barcelona (UAB). +// This work is licensed under the terms of the MIT license. +// For a copy, see . + +#pragma once +#include +#include "carla/ros2/types/msg/Header.h" +#include "carla/ros2/types/msg/PoseWithCovariance.h" +#include "carla/ros2/types/msg/TwistWithCovariance.h" + +namespace carla { +namespace ros2 { +namespace msg { + +struct Odometry { + Header header; + std::string child_frame_id; + PoseWithCovariance pose; + TwistWithCovariance twist; +}; + +} // namespace msg +} // namespace ros2 +} // namespace carla diff --git a/LibCarla/source/carla/ros2/types/msg/Point.h b/LibCarla/source/carla/ros2/types/msg/Point.h new file mode 100644 index 00000000000..b731e4a19df --- /dev/null +++ b/LibCarla/source/carla/ros2/types/msg/Point.h @@ -0,0 +1,19 @@ +// Copyright (c) 2025 Computer Vision Center (CVC) at the Universitat Autonoma de Barcelona (UAB). +// This work is licensed under the terms of the MIT license. +// For a copy, see . + +#pragma once + +namespace carla { +namespace ros2 { +namespace msg { + +struct Point { + double x = 0.0; + double y = 0.0; + double z = 0.0; +}; + +} // namespace msg +} // namespace ros2 +} // namespace carla diff --git a/LibCarla/source/carla/ros2/types/msg/Point32.h b/LibCarla/source/carla/ros2/types/msg/Point32.h new file mode 100644 index 00000000000..f3ee652542d --- /dev/null +++ b/LibCarla/source/carla/ros2/types/msg/Point32.h @@ -0,0 +1,19 @@ +// Copyright (c) 2025 Computer Vision Center (CVC) at the Universitat Autonoma de Barcelona (UAB). +// This work is licensed under the terms of the MIT license. +// For a copy, see . + +#pragma once + +namespace carla { +namespace ros2 { +namespace msg { + +struct Point32 { + float x = 0.0f; + float y = 0.0f; + float z = 0.0f; +}; + +} // namespace msg +} // namespace ros2 +} // namespace carla diff --git a/LibCarla/source/carla/ros2/types/msg/PointCloud2.h b/LibCarla/source/carla/ros2/types/msg/PointCloud2.h new file mode 100644 index 00000000000..4cd62edb0d7 --- /dev/null +++ b/LibCarla/source/carla/ros2/types/msg/PointCloud2.h @@ -0,0 +1,29 @@ +// Copyright (c) 2025 Computer Vision Center (CVC) at the Universitat Autonoma de Barcelona (UAB). +// This work is licensed under the terms of the MIT license. +// For a copy, see . + +#pragma once +#include +#include +#include "carla/ros2/types/msg/Header.h" +#include "carla/ros2/types/msg/PointField.h" + +namespace carla { +namespace ros2 { +namespace msg { + +struct PointCloud2 { + Header header; + uint32_t height = 0; + uint32_t width = 0; + std::vector fields; + bool is_bigendian = false; + uint32_t point_step = 0; + uint32_t row_step = 0; + std::vector data; + bool is_dense = false; +}; + +} // namespace msg +} // namespace ros2 +} // namespace carla diff --git a/LibCarla/source/carla/ros2/types/msg/PointField.h b/LibCarla/source/carla/ros2/types/msg/PointField.h new file mode 100644 index 00000000000..85d9f8da983 --- /dev/null +++ b/LibCarla/source/carla/ros2/types/msg/PointField.h @@ -0,0 +1,31 @@ +// Copyright (c) 2025 Computer Vision Center (CVC) at the Universitat Autonoma de Barcelona (UAB). +// This work is licensed under the terms of the MIT license. +// For a copy, see . + +#pragma once +#include +#include + +namespace carla { +namespace ros2 { +namespace msg { + +struct PointField { + static const uint8_t INT8 = 1; + static const uint8_t UINT8 = 2; + static const uint8_t INT16 = 3; + static const uint8_t UINT16 = 4; + static const uint8_t INT32 = 5; + static const uint8_t UINT32 = 6; + static const uint8_t FLOAT32 = 7; + static const uint8_t FLOAT64 = 8; + + std::string name; + uint32_t offset = 0; + uint8_t datatype = 0; + uint32_t count = 0; +}; + +} // namespace msg +} // namespace ros2 +} // namespace carla diff --git a/LibCarla/source/carla/ros2/types/msg/Pose.h b/LibCarla/source/carla/ros2/types/msg/Pose.h new file mode 100644 index 00000000000..bf3c81b869a --- /dev/null +++ b/LibCarla/source/carla/ros2/types/msg/Pose.h @@ -0,0 +1,20 @@ +// Copyright (c) 2025 Computer Vision Center (CVC) at the Universitat Autonoma de Barcelona (UAB). +// This work is licensed under the terms of the MIT license. +// For a copy, see . + +#pragma once +#include "carla/ros2/types/msg/Point.h" +#include "carla/ros2/types/msg/Quaternion.h" + +namespace carla { +namespace ros2 { +namespace msg { + +struct Pose { + Point position; + Quaternion orientation; +}; + +} // namespace msg +} // namespace ros2 +} // namespace carla diff --git a/LibCarla/source/carla/ros2/types/msg/PoseWithCovariance.h b/LibCarla/source/carla/ros2/types/msg/PoseWithCovariance.h new file mode 100644 index 00000000000..3570f8f0a50 --- /dev/null +++ b/LibCarla/source/carla/ros2/types/msg/PoseWithCovariance.h @@ -0,0 +1,20 @@ +// Copyright (c) 2025 Computer Vision Center (CVC) at the Universitat Autonoma de Barcelona (UAB). +// This work is licensed under the terms of the MIT license. +// For a copy, see . + +#pragma once +#include +#include "carla/ros2/types/msg/Pose.h" + +namespace carla { +namespace ros2 { +namespace msg { + +struct PoseWithCovariance { + Pose pose; + std::array covariance = {}; +}; + +} // namespace msg +} // namespace ros2 +} // namespace carla diff --git a/LibCarla/source/carla/ros2/types/msg/Quaternion.h b/LibCarla/source/carla/ros2/types/msg/Quaternion.h new file mode 100644 index 00000000000..59cecdf4725 --- /dev/null +++ b/LibCarla/source/carla/ros2/types/msg/Quaternion.h @@ -0,0 +1,20 @@ +// Copyright (c) 2025 Computer Vision Center (CVC) at the Universitat Autonoma de Barcelona (UAB). +// This work is licensed under the terms of the MIT license. +// For a copy, see . + +#pragma once + +namespace carla { +namespace ros2 { +namespace msg { + +struct Quaternion { + double x = 0.0; + double y = 0.0; + double z = 0.0; + double w = 0.0; +}; + +} // namespace msg +} // namespace ros2 +} // namespace carla diff --git a/LibCarla/source/carla/ros2/types/msg/RegionOfInterest.h b/LibCarla/source/carla/ros2/types/msg/RegionOfInterest.h new file mode 100644 index 00000000000..0a21608c655 --- /dev/null +++ b/LibCarla/source/carla/ros2/types/msg/RegionOfInterest.h @@ -0,0 +1,22 @@ +// Copyright (c) 2025 Computer Vision Center (CVC) at the Universitat Autonoma de Barcelona (UAB). +// This work is licensed under the terms of the MIT license. +// For a copy, see . + +#pragma once +#include + +namespace carla { +namespace ros2 { +namespace msg { + +struct RegionOfInterest { + uint32_t x_offset = 0; + uint32_t y_offset = 0; + uint32_t height = 0; + uint32_t width = 0; + bool do_rectify = false; +}; + +} // namespace msg +} // namespace ros2 +} // namespace carla diff --git a/LibCarla/source/carla/ros2/types/msg/String.h b/LibCarla/source/carla/ros2/types/msg/String.h new file mode 100644 index 00000000000..81a50e40248 --- /dev/null +++ b/LibCarla/source/carla/ros2/types/msg/String.h @@ -0,0 +1,18 @@ +// Copyright (c) 2025 Computer Vision Center (CVC) at the Universitat Autonoma de Barcelona (UAB). +// This work is licensed under the terms of the MIT license. +// For a copy, see . + +#pragma once +#include + +namespace carla { +namespace ros2 { +namespace msg { + +struct String { + std::string data; +}; + +} // namespace msg +} // namespace ros2 +} // namespace carla diff --git a/LibCarla/source/carla/ros2/types/msg/TF2Error.h b/LibCarla/source/carla/ros2/types/msg/TF2Error.h new file mode 100644 index 00000000000..33f4bf46e8f --- /dev/null +++ b/LibCarla/source/carla/ros2/types/msg/TF2Error.h @@ -0,0 +1,20 @@ +// Copyright (c) 2025 Computer Vision Center (CVC) at the Universitat Autonoma de Barcelona (UAB). +// This work is licensed under the terms of the MIT license. +// For a copy, see . + +#pragma once +#include +#include + +namespace carla { +namespace ros2 { +namespace msg { + +struct TF2Error { + uint8_t error = 0; + std::string error_string; +}; + +} // namespace msg +} // namespace ros2 +} // namespace carla diff --git a/LibCarla/source/carla/ros2/types/msg/TFMessage.h b/LibCarla/source/carla/ros2/types/msg/TFMessage.h new file mode 100644 index 00000000000..54636dffa79 --- /dev/null +++ b/LibCarla/source/carla/ros2/types/msg/TFMessage.h @@ -0,0 +1,19 @@ +// Copyright (c) 2025 Computer Vision Center (CVC) at the Universitat Autonoma de Barcelona (UAB). +// This work is licensed under the terms of the MIT license. +// For a copy, see . + +#pragma once +#include +#include "carla/ros2/types/msg/TransformStamped.h" + +namespace carla { +namespace ros2 { +namespace msg { + +struct TFMessage { + std::vector transforms; +}; + +} // namespace msg +} // namespace ros2 +} // namespace carla diff --git a/LibCarla/source/carla/ros2/types/msg/Time.h b/LibCarla/source/carla/ros2/types/msg/Time.h new file mode 100644 index 00000000000..4183173ee83 --- /dev/null +++ b/LibCarla/source/carla/ros2/types/msg/Time.h @@ -0,0 +1,19 @@ +// Copyright (c) 2025 Computer Vision Center (CVC) at the Universitat Autonoma de Barcelona (UAB). +// This work is licensed under the terms of the MIT license. +// For a copy, see . + +#pragma once +#include + +namespace carla { +namespace ros2 { +namespace msg { + +struct Time { + int32_t sec = 0; + uint32_t nanosec = 0; +}; + +} // namespace msg +} // namespace ros2 +} // namespace carla diff --git a/LibCarla/source/carla/ros2/types/msg/Transform.h b/LibCarla/source/carla/ros2/types/msg/Transform.h new file mode 100644 index 00000000000..99ec1d2b9c7 --- /dev/null +++ b/LibCarla/source/carla/ros2/types/msg/Transform.h @@ -0,0 +1,20 @@ +// Copyright (c) 2025 Computer Vision Center (CVC) at the Universitat Autonoma de Barcelona (UAB). +// This work is licensed under the terms of the MIT license. +// For a copy, see . + +#pragma once +#include "carla/ros2/types/msg/Vector3.h" +#include "carla/ros2/types/msg/Quaternion.h" + +namespace carla { +namespace ros2 { +namespace msg { + +struct Transform { + Vector3 translation; + Quaternion rotation; +}; + +} // namespace msg +} // namespace ros2 +} // namespace carla diff --git a/LibCarla/source/carla/ros2/types/msg/TransformStamped.h b/LibCarla/source/carla/ros2/types/msg/TransformStamped.h new file mode 100644 index 00000000000..cb9d471c3bd --- /dev/null +++ b/LibCarla/source/carla/ros2/types/msg/TransformStamped.h @@ -0,0 +1,22 @@ +// Copyright (c) 2025 Computer Vision Center (CVC) at the Universitat Autonoma de Barcelona (UAB). +// This work is licensed under the terms of the MIT license. +// For a copy, see . + +#pragma once +#include +#include "carla/ros2/types/msg/Header.h" +#include "carla/ros2/types/msg/Transform.h" + +namespace carla { +namespace ros2 { +namespace msg { + +struct TransformStamped { + Header header; + std::string child_frame_id; + Transform transform; +}; + +} // namespace msg +} // namespace ros2 +} // namespace carla diff --git a/LibCarla/source/carla/ros2/types/msg/Twist.h b/LibCarla/source/carla/ros2/types/msg/Twist.h new file mode 100644 index 00000000000..ed2d8d42e36 --- /dev/null +++ b/LibCarla/source/carla/ros2/types/msg/Twist.h @@ -0,0 +1,19 @@ +// Copyright (c) 2025 Computer Vision Center (CVC) at the Universitat Autonoma de Barcelona (UAB). +// This work is licensed under the terms of the MIT license. +// For a copy, see . + +#pragma once +#include "carla/ros2/types/msg/Vector3.h" + +namespace carla { +namespace ros2 { +namespace msg { + +struct Twist { + Vector3 linear; + Vector3 angular; +}; + +} // namespace msg +} // namespace ros2 +} // namespace carla diff --git a/LibCarla/source/carla/ros2/types/msg/TwistWithCovariance.h b/LibCarla/source/carla/ros2/types/msg/TwistWithCovariance.h new file mode 100644 index 00000000000..6e00857d6c1 --- /dev/null +++ b/LibCarla/source/carla/ros2/types/msg/TwistWithCovariance.h @@ -0,0 +1,20 @@ +// Copyright (c) 2025 Computer Vision Center (CVC) at the Universitat Autonoma de Barcelona (UAB). +// This work is licensed under the terms of the MIT license. +// For a copy, see . + +#pragma once +#include +#include "carla/ros2/types/msg/Twist.h" + +namespace carla { +namespace ros2 { +namespace msg { + +struct TwistWithCovariance { + Twist twist; + std::array covariance = {}; +}; + +} // namespace msg +} // namespace ros2 +} // namespace carla diff --git a/LibCarla/source/carla/ros2/types/msg/Vector3.h b/LibCarla/source/carla/ros2/types/msg/Vector3.h new file mode 100644 index 00000000000..1b879020e11 --- /dev/null +++ b/LibCarla/source/carla/ros2/types/msg/Vector3.h @@ -0,0 +1,19 @@ +// Copyright (c) 2025 Computer Vision Center (CVC) at the Universitat Autonoma de Barcelona (UAB). +// This work is licensed under the terms of the MIT license. +// For a copy, see . + +#pragma once + +namespace carla { +namespace ros2 { +namespace msg { + +struct Vector3 { + double x = 0.0; + double y = 0.0; + double z = 0.0; +}; + +} // namespace msg +} // namespace ros2 +} // namespace carla diff --git a/LibCarla/source/test/server/test_dds_middleware.cpp b/LibCarla/source/test/server/test_dds_middleware.cpp new file mode 100644 index 00000000000..a4563b94c81 --- /dev/null +++ b/LibCarla/source/test/server/test_dds_middleware.cpp @@ -0,0 +1,490 @@ +// Copyright (c) 2025 Computer Vision Center (CVC) at the Universitat Autonoma de Barcelona (UAB). +// This work is licensed under the terms of the MIT license. +// For a copy, see . + +// Must be defined before any includes to suppress real DDS auto-includes +// while keeping CARLA_ROS2_DDS_FASTDDS compile-time checks active. +#define CARLA_ROS2_DDS_TESTING +#define CARLA_ROS2_DDS_FASTDDS + +#include "test.h" + +#include +#include +#include +#include +#include +#include + +#include +#include + +using namespace carla::ros2; + +// ========================================================================== +// Test infrastructure +// ========================================================================== + +struct TestMsg { + int value{0}; +}; + +struct TestPubTraits { + using msg_type = TestMsg; +}; + +struct TestSubTraits { + using msg_type = TestMsg; +}; + +// -- Mock publisher middleware ------------------------------------------------ + +class MockPublisherMiddleware : public IDDSPublisherMiddleware { + public: + bool init_return_value{true}; + bool publish_return_value{true}; + bool alive{true}; + + bool init_called{false}; + bool publish_called{false}; + std::string last_topic_name; + void* last_published_data{nullptr}; + + bool Init(const std::string& topic_name) override { + init_called = true; + last_topic_name = topic_name; + return init_return_value; + } + + bool Publish(void* message_data) override { + publish_called = true; + last_published_data = message_data; + return publish_return_value; + } + + bool IsAlive() const override { return alive; } + std::string GetTopicName() const override { return last_topic_name; } +}; + +// -- Mock subscriber middleware ----------------------------------------------- + +class MockSubscriberMiddleware : public IDDSSubscriberMiddleware { + public: + bool init_return_value{true}; + bool alive{true}; + + bool init_called{false}; + std::string last_topic_name; + void* stored_message_ptr{nullptr}; + bool* stored_flag_ptr{nullptr}; + + bool Init( + const std::string& topic_name, + void* message_ptr, + bool* new_message_flag) override { + init_called = true; + last_topic_name = topic_name; + stored_message_ptr = message_ptr; + stored_flag_ptr = new_message_flag; + return init_return_value; + } + + bool IsAlive() const override { return alive; } + std::string GetTopicName() const override { return last_topic_name; } +}; + +// -- Factory fixture (resets static state) ------------------------------------ + +class DDSMiddlewareFactoryFixture : public ::testing::Test { + protected: + void SetUp() override { + DDSMiddlewareFactory::SetMiddleware(DDSMiddleware::FastDDS); + } +}; + +// ========================================================================== +// Group 1: dds_middleware_enum (2 tests) +// ========================================================================== + +TEST(dds_middleware_enum, values_exist) { + DDSMiddleware mw = DDSMiddleware::FastDDS; + EXPECT_EQ(static_cast(mw), 0); +} + +TEST(dds_middleware_enum, switch_covers_all) { + DDSMiddleware mw = DDSMiddleware::FastDDS; + bool covered = false; + switch (mw) { + case DDSMiddleware::FastDDS: + covered = true; + break; + } + EXPECT_TRUE(covered); +} + +// ========================================================================== +// Group 2: dds_middleware_to_string (3 tests) +// ========================================================================== + +TEST(dds_middleware_to_string, fastdds_returns_correct_string) { + EXPECT_STREQ(DDSMiddlewareToString(DDSMiddleware::FastDDS), "FastDDS"); +} + +TEST(dds_middleware_to_string, result_is_not_null) { + const char* result = DDSMiddlewareToString(DDSMiddleware::FastDDS); + ASSERT_NE(result, nullptr); +} + +TEST(dds_middleware_to_string, result_is_not_empty) { + const char* result = DDSMiddlewareToString(DDSMiddleware::FastDDS); + EXPECT_STRNE(result, ""); +} + +// ========================================================================== +// Group 3: dds_middleware_from_string (5 tests) +// ========================================================================== + +TEST(dds_middleware_from_string, fastdds_lowercase_valid) { + auto result = DDSMiddlewareFromString("fastdds"); + EXPECT_TRUE(result.valid); + EXPECT_EQ(result.middleware, DDSMiddleware::FastDDS); +} + +TEST(dds_middleware_from_string, unknown_string_invalid) { + auto result = DDSMiddlewareFromString("cyclonedds"); + EXPECT_FALSE(result.valid); +} + +TEST(dds_middleware_from_string, empty_string_invalid) { + auto result = DDSMiddlewareFromString(""); + EXPECT_FALSE(result.valid); +} + +TEST(dds_middleware_from_string, uppercase_rejected) { + auto result = DDSMiddlewareFromString("FastDDS"); + EXPECT_FALSE(result.valid); +} + +TEST(dds_middleware_from_string, partial_match_rejected) { + auto result = DDSMiddlewareFromString("fast"); + EXPECT_FALSE(result.valid); +} + +// ========================================================================== +// Group 4: dds_middleware_available (2 tests) +// ========================================================================== + +TEST(dds_middleware_available, fastdds_available) { + EXPECT_TRUE( + DDSMiddlewareFactory::IsMiddlewareAvailable(DDSMiddleware::FastDDS)); +} + +TEST(dds_middleware_available, available_string_contains_fastdds) { + std::string available = GetAvailableMiddlewareString(); + EXPECT_NE(available.find("FastDDS"), std::string::npos); +} + +// ========================================================================== +// Group 5: dds_middleware_type_name (4 tests) +// ========================================================================== + +TEST(dds_middleware_type_name, bare_name) { + EXPECT_EQ(ToROS2DDSTypeName("Image"), "dds_::Image_"); +} + +TEST(dds_middleware_type_name, fully_qualified) { + EXPECT_EQ( + ToROS2DDSTypeName("sensor_msgs::msg::Image"), + "sensor_msgs::msg::dds_::Image_"); +} + +TEST(dds_middleware_type_name, single_namespace) { + EXPECT_EQ(ToROS2DDSTypeName("msg::Image"), "msg::dds_::Image_"); +} + +TEST(dds_middleware_type_name, empty_string) { + EXPECT_EQ(ToROS2DDSTypeName(""), "dds_::_"); +} + +// ========================================================================== +// Group 6: DDSMiddlewareFactoryFixture (5 tests) +// ========================================================================== + +TEST_F(DDSMiddlewareFactoryFixture, set_and_get_middleware) { + DDSMiddlewareFactory::SetMiddleware(DDSMiddleware::FastDDS); + EXPECT_EQ(DDSMiddlewareFactory::GetMiddleware(), DDSMiddleware::FastDDS); +} + +TEST_F(DDSMiddlewareFactoryFixture, default_is_fastdds) { + EXPECT_EQ(DDSMiddlewareFactory::GetMiddleware(), DDSMiddleware::FastDDS); +} + +TEST_F(DDSMiddlewareFactoryFixture, is_middleware_available_fastdds) { + EXPECT_TRUE( + DDSMiddlewareFactory::IsMiddlewareAvailable(DDSMiddleware::FastDDS)); +} + +TEST_F(DDSMiddlewareFactoryFixture, resolve_available_middleware) { + auto resolution = + DDSMiddlewareFactory::ResolveMiddleware(DDSMiddleware::FastDDS); + EXPECT_TRUE(resolution.success); + EXPECT_EQ(resolution.middleware, DDSMiddleware::FastDDS); +} + +TEST_F(DDSMiddlewareFactoryFixture, factory_available_string) { + std::string available = DDSMiddlewareFactory::GetAvailableMiddlewareString(); + EXPECT_NE(available.find("FastDDS"), std::string::npos); +} + +// ========================================================================== +// Group 7: dds_publisher_interface (5 tests) +// ========================================================================== + +TEST(dds_publisher_interface, mock_init_success) { + MockPublisherMiddleware mock; + mock.init_return_value = true; + EXPECT_TRUE(mock.Init("rt/test_topic")); + EXPECT_TRUE(mock.init_called); + EXPECT_EQ(mock.last_topic_name, "rt/test_topic"); +} + +TEST(dds_publisher_interface, mock_init_failure) { + MockPublisherMiddleware mock; + mock.init_return_value = false; + EXPECT_FALSE(mock.Init("rt/test_topic")); + EXPECT_TRUE(mock.init_called); +} + +TEST(dds_publisher_interface, mock_publish_records_data) { + MockPublisherMiddleware mock; + TestMsg msg; + msg.value = 42; + EXPECT_TRUE(mock.Publish(&msg)); + EXPECT_TRUE(mock.publish_called); + EXPECT_EQ(mock.last_published_data, &msg); +} + +TEST(dds_publisher_interface, mock_topic_name) { + MockPublisherMiddleware mock; + mock.Init("rt/camera/image"); + EXPECT_EQ(mock.GetTopicName(), "rt/camera/image"); +} + +TEST(dds_publisher_interface, mock_is_alive) { + MockPublisherMiddleware mock; + mock.alive = true; + EXPECT_TRUE(mock.IsAlive()); + mock.alive = false; + EXPECT_FALSE(mock.IsAlive()); +} + +// ========================================================================== +// Group 8: dds_subscriber_interface (5 tests) +// ========================================================================== + +TEST(dds_subscriber_interface, mock_init_stores_pointers) { + MockSubscriberMiddleware mock; + TestMsg msg; + bool flag = false; + EXPECT_TRUE(mock.Init("rt/test_topic", &msg, &flag)); + EXPECT_TRUE(mock.init_called); + EXPECT_EQ(mock.stored_message_ptr, &msg); + EXPECT_EQ(mock.stored_flag_ptr, &flag); +} + +TEST(dds_subscriber_interface, mock_init_failure) { + MockSubscriberMiddleware mock; + mock.init_return_value = false; + TestMsg msg; + bool flag = false; + EXPECT_FALSE(mock.Init("rt/test_topic", &msg, &flag)); +} + +TEST(dds_subscriber_interface, mock_topic_name) { + MockSubscriberMiddleware mock; + TestMsg msg; + bool flag = false; + mock.Init("rt/lidar/points", &msg, &flag); + EXPECT_EQ(mock.GetTopicName(), "rt/lidar/points"); +} + +TEST(dds_subscriber_interface, mock_is_alive) { + MockSubscriberMiddleware mock; + mock.alive = true; + EXPECT_TRUE(mock.IsAlive()); + mock.alive = false; + EXPECT_FALSE(mock.IsAlive()); +} + +TEST(dds_subscriber_interface, mock_simulates_message_receipt) { + MockSubscriberMiddleware mock; + TestMsg msg; + bool flag = false; + mock.Init("rt/test", &msg, &flag); + ASSERT_NE(mock.stored_message_ptr, nullptr); + ASSERT_NE(mock.stored_flag_ptr, nullptr); + auto* typed_ptr = static_cast(mock.stored_message_ptr); + typed_ptr->value = 99; + *mock.stored_flag_ptr = true; + EXPECT_EQ(msg.value, 99); + EXPECT_TRUE(flag); +} + +// ========================================================================== +// Group 9: publisher_impl (7 tests) +// ========================================================================== + +TEST(publisher_impl, get_message_returns_pointer) { + PublisherImpl pub; + TestMsg* msg = pub.GetMessage(); + ASSERT_NE(msg, nullptr); + msg->value = 7; + EXPECT_EQ(pub.GetMessage()->value, 7); +} + +TEST(publisher_impl, init_delegates_to_middleware) { + PublisherImpl pub; + auto* mock = new MockPublisherMiddleware(); + pub.SetMiddlewareForTesting( + std::unique_ptr(mock)); + EXPECT_TRUE(pub.Init("rt/test_topic")); + EXPECT_TRUE(mock->init_called); + EXPECT_EQ(mock->last_topic_name, "rt/test_topic"); +} + +TEST(publisher_impl, publish_delegates_to_middleware) { + PublisherImpl pub; + auto* mock = new MockPublisherMiddleware(); + pub.SetMiddlewareForTesting( + std::unique_ptr(mock)); + pub.Init("rt/test_topic"); + EXPECT_TRUE(pub.Publish()); + EXPECT_TRUE(mock->publish_called); + EXPECT_EQ(mock->last_published_data, pub.GetMessage()); +} + +TEST(publisher_impl, publish_before_init_fails) { + PublisherImpl pub; + ::testing::internal::CaptureStderr(); + EXPECT_FALSE(pub.Publish()); + ::testing::internal::GetCapturedStderr(); +} + +TEST(publisher_impl, is_alive_delegates) { + PublisherImpl pub; + auto* mock = new MockPublisherMiddleware(); + mock->alive = true; + pub.SetMiddlewareForTesting( + std::unique_ptr(mock)); + pub.Init("rt/test_topic"); + EXPECT_TRUE(pub.IsAlive()); + mock->alive = false; + EXPECT_FALSE(pub.IsAlive()); +} + +TEST(publisher_impl, topic_name_delegates) { + PublisherImpl pub; + auto* mock = new MockPublisherMiddleware(); + pub.SetMiddlewareForTesting( + std::unique_ptr(mock)); + pub.Init("rt/camera/image"); + EXPECT_EQ(pub.GetTopicName(), "rt/camera/image"); +} + +TEST(publisher_impl, data_flows_through_publish) { + PublisherImpl pub; + auto* mock = new MockPublisherMiddleware(); + pub.SetMiddlewareForTesting( + std::unique_ptr(mock)); + pub.Init("rt/test_topic"); + + pub.GetMessage()->value = 42; + pub.Publish(); + + ASSERT_NE(mock->last_published_data, nullptr); + auto* published = static_cast(mock->last_published_data); + EXPECT_EQ(published->value, 42); +} + +// ========================================================================== +// Group 10: subscriber_impl (7 tests) +// ========================================================================== + +TEST(subscriber_impl, has_new_message_initially_false) { + SubscriberImpl sub; + EXPECT_FALSE(sub.HasNewMessage()); +} + +TEST(subscriber_impl, init_delegates_to_middleware) { + SubscriberImpl sub; + auto* mock = new MockSubscriberMiddleware(); + sub.SetMiddlewareForTesting( + std::unique_ptr(mock)); + EXPECT_TRUE(sub.Init("rt/test_topic")); + EXPECT_TRUE(mock->init_called); + EXPECT_EQ(mock->last_topic_name, "rt/test_topic"); + EXPECT_NE(mock->stored_message_ptr, nullptr); + EXPECT_NE(mock->stored_flag_ptr, nullptr); +} + +TEST(subscriber_impl, get_message_clears_flag) { + SubscriberImpl sub; + auto* mock = new MockSubscriberMiddleware(); + sub.SetMiddlewareForTesting( + std::unique_ptr(mock)); + sub.Init("rt/test_topic"); + + TestMsg msg; + msg.value = 77; + sub.SimulateMessageReceiptForTesting(msg); + EXPECT_TRUE(sub.HasNewMessage()); + + TestMsg retrieved = sub.GetMessage(); + EXPECT_EQ(retrieved.value, 77); + EXPECT_FALSE(sub.HasNewMessage()); +} + +TEST(subscriber_impl, is_alive_delegates) { + SubscriberImpl sub; + auto* mock = new MockSubscriberMiddleware(); + mock->alive = true; + sub.SetMiddlewareForTesting( + std::unique_ptr(mock)); + sub.Init("rt/test_topic"); + EXPECT_TRUE(sub.IsAlive()); + mock->alive = false; + EXPECT_FALSE(sub.IsAlive()); +} + +TEST(subscriber_impl, topic_name_delegates) { + SubscriberImpl sub; + auto* mock = new MockSubscriberMiddleware(); + sub.SetMiddlewareForTesting( + std::unique_ptr(mock)); + sub.Init("rt/lidar/points"); + EXPECT_EQ(sub.GetTopicName(), "rt/lidar/points"); +} + +TEST(subscriber_impl, simulate_message_receipt) { + SubscriberImpl sub; + auto* mock = new MockSubscriberMiddleware(); + sub.SetMiddlewareForTesting( + std::unique_ptr(mock)); + sub.Init("rt/test_topic"); + + EXPECT_FALSE(sub.HasNewMessage()); + TestMsg msg; + msg.value = 123; + sub.SimulateMessageReceiptForTesting(msg); + EXPECT_TRUE(sub.HasNewMessage()); + EXPECT_EQ(sub.GetMessage().value, 123); +} + +TEST(subscriber_impl, init_failure_propagated) { + SubscriberImpl sub; + auto* mock = new MockSubscriberMiddleware(); + mock->init_return_value = false; + sub.SetMiddlewareForTesting( + std::unique_ptr(mock)); + EXPECT_FALSE(sub.Init("rt/test_topic")); +} From 98b6f7aa7a7e01ee8c1f95a4c408f16b32c4dd42 Mon Sep 17 00:00:00 2001 From: Jesus Armando Anaya Date: Sun, 29 Mar 2026 23:15:30 -0700 Subject: [PATCH 3/4] feat(LibCarla/ros2): migrate publishers and subscribers to POD message types Switch all concrete publishers and subscribers from FastDDS-generated types to middleware-neutral POD structs (carla::ros2::msg::*). Remove identity TypeMap specializations and generic identity converters from FastDDSTypeMap, leaving only real POD-to-FastDDS mappings. Convert FastDDS accessor syntax (.field(val) / .field()) to direct member access (.field = val / .field). Fix DVS descriptor4 bug where fields were set on descriptor3. Add log_warning on Init failure for all publishers and subscribers. Add missing copyright headers to subscriber CPP files. --- .../carla/ros2/dds/fastdds/FastDDSTypeMap.h | 72 +--------------- .../ros2/publishers/CarlaCameraPublisher.cpp | 54 ++++++------ .../ros2/publishers/CarlaCameraPublisher.h | 20 ++--- .../ros2/publishers/CarlaClockPublisher.cpp | 4 +- .../ros2/publishers/CarlaClockPublisher.h | 10 +-- .../publishers/CarlaCollisionPublisher.cpp | 14 +-- .../ros2/publishers/CarlaCollisionPublisher.h | 10 +-- .../ros2/publishers/CarlaDVSPublisher.cpp | 47 +++++----- .../carla/ros2/publishers/CarlaDVSPublisher.h | 2 +- .../ros2/publishers/CarlaGNSSPublisher.cpp | 12 +-- .../ros2/publishers/CarlaGNSSPublisher.h | 10 +-- .../ros2/publishers/CarlaIMUPublisher.cpp | 26 +++--- .../carla/ros2/publishers/CarlaIMUPublisher.h | 10 +-- .../ros2/publishers/CarlaLidarPublisher.cpp | 50 +++++------ .../ros2/publishers/CarlaLidarPublisher.h | 2 +- .../publishers/CarlaPointCloudPublisher.cpp | 22 ++--- .../publishers/CarlaPointCloudPublisher.h | 12 +-- .../ros2/publishers/CarlaRadarPublisher.cpp | 86 +++++++++---------- .../ros2/publishers/CarlaRadarPublisher.h | 2 +- .../CarlaSemanticLidarPublisher.cpp | 74 ++++++++-------- .../publishers/CarlaSemanticLidarPublisher.h | 2 +- .../publishers/CarlaTransformPublisher.cpp | 33 ++++--- .../ros2/publishers/CarlaTransformPublisher.h | 15 ++-- .../AckermannControlSubscriber.cpp | 14 +-- .../subscribers/AckermannControlSubscriber.h | 11 +-- .../CarlaEgoVehicleControlSubscriber.cpp | 18 ++-- .../CarlaEgoVehicleControlSubscriber.h | 11 +-- 27 files changed, 294 insertions(+), 349 deletions(-) diff --git a/LibCarla/source/carla/ros2/dds/fastdds/FastDDSTypeMap.h b/LibCarla/source/carla/ros2/dds/fastdds/FastDDSTypeMap.h index 1a9ad1935a1..3770477df30 100644 --- a/LibCarla/source/carla/ros2/dds/fastdds/FastDDSTypeMap.h +++ b/LibCarla/source/carla/ros2/dds/fastdds/FastDDSTypeMap.h @@ -47,76 +47,8 @@ namespace ros2 { template struct FastDDSTypeMap; // ============================================================ -// Identity specializations (msg_type IS the FastDDS type) -// Used by current publishers/subscribers until they are migrated -// to POD message types in PR #2b. -// ============================================================ - -template<> struct FastDDSTypeMap { - using fastdds_type = sensor_msgs::msg::NavSatFix; - using fastdds_pubsub_type = sensor_msgs::msg::NavSatFixPubSubType; -}; - -template<> struct FastDDSTypeMap { - using fastdds_type = sensor_msgs::msg::Image; - using fastdds_pubsub_type = sensor_msgs::msg::ImagePubSubType; -}; - -template<> struct FastDDSTypeMap { - using fastdds_type = sensor_msgs::msg::CameraInfo; - using fastdds_pubsub_type = sensor_msgs::msg::CameraInfoPubSubType; -}; - -template<> struct FastDDSTypeMap { - using fastdds_type = sensor_msgs::msg::Imu; - using fastdds_pubsub_type = sensor_msgs::msg::ImuPubSubType; -}; - -template<> struct FastDDSTypeMap { - using fastdds_type = sensor_msgs::msg::PointCloud2; - using fastdds_pubsub_type = sensor_msgs::msg::PointCloud2PubSubType; -}; - -template<> struct FastDDSTypeMap { - using fastdds_type = rosgraph::msg::Clock; - using fastdds_pubsub_type = rosgraph::msg::ClockPubSubType; -}; - -template<> struct FastDDSTypeMap { - using fastdds_type = tf2_msgs::msg::TFMessage; - using fastdds_pubsub_type = tf2_msgs::msg::TFMessagePubSubType; -}; - -template<> struct FastDDSTypeMap { - using fastdds_type = carla_msgs::msg::CarlaCollisionEvent; - using fastdds_pubsub_type = carla_msgs::msg::CarlaCollisionEventPubSubType; -}; - -template<> struct FastDDSTypeMap { - using fastdds_type = carla_msgs::msg::CarlaEgoVehicleControl; - using fastdds_pubsub_type = carla_msgs::msg::CarlaEgoVehicleControlPubSubType; -}; - -template<> struct FastDDSTypeMap { - using fastdds_type = ackermann_msgs::msg::AckermannDriveStamped; - using fastdds_pubsub_type = ackermann_msgs::msg::AckermannDriveStampedPubSubType; -}; - -/// Identity conversion: copy src to dst when both types are the same. -/// Used by publishers/subscribers that have not yet migrated to POD types. -template -inline void to_fastdds(const T& src, T& dst) { - dst = src; -} - -template -inline void from_fastdds(const T& src, T& dst) { - dst = src; -} - -// ============================================================ -// Real specializations (POD msg types -> FastDDS types) -// These map backend-neutral POD structs to FastDDS-generated types. +// Specializations (POD msg types -> FastDDS types) +// These map middleware-neutral POD structs to FastDDS-generated types. // Conversion functions are in types/FastDDSConversions.h. // ============================================================ diff --git a/LibCarla/source/carla/ros2/publishers/CarlaCameraPublisher.cpp b/LibCarla/source/carla/ros2/publishers/CarlaCameraPublisher.cpp index edb98ff4eae..cb339516b2b 100644 --- a/LibCarla/source/carla/ros2/publishers/CarlaCameraPublisher.cpp +++ b/LibCarla/source/carla/ros2/publishers/CarlaCameraPublisher.cpp @@ -15,46 +15,46 @@ std::vector CarlaCameraPublisher::ComputeImage(uint32_t height, uint32_ bool CarlaCameraPublisher::WriteCameraInfo(int32_t seconds, uint32_t nanoseconds, uint32_t x_offset, uint32_t y_offset, uint32_t height, uint32_t width, float fov, bool do_rectify) { - _impl_camera_info->GetMessage()->header().stamp().sec(seconds); - _impl_camera_info->GetMessage()->header().stamp().nanosec(nanoseconds); - _impl_camera_info->GetMessage()->header().frame_id(GetFrameId()); + _impl_camera_info->GetMessage()->header.stamp.sec = seconds; + _impl_camera_info->GetMessage()->header.stamp.nanosec = nanoseconds; + _impl_camera_info->GetMessage()->header.frame_id = GetFrameId(); const double cx = static_cast(width) / 2.0; const double cy = static_cast(height) / 2.0; const double fx = static_cast(width) / (2.0 * std::tan(fov * M_PI / 360.0)); const double fy = fx; - _impl_camera_info->GetMessage()->height(height); - _impl_camera_info->GetMessage()->width(width); - _impl_camera_info->GetMessage()->distortion_model("plumb_bob"); - _impl_camera_info->GetMessage()->D({ 0.0, 0.0, 0.0, 0.0, 0.0 }); - _impl_camera_info->GetMessage()->k({fx, 0.0, cx, 0.0, fy, cy, 0.0, 0.0, 1.0}); - _impl_camera_info->GetMessage()->r({ 1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0 }); - _impl_camera_info->GetMessage()->p({fx, 0.0, cx, 0.0, 0.0, fy, cy, 0.0, 0.0, 0.0, 1.0, 0.0}); - _impl_camera_info->GetMessage()->binning_x(0); - _impl_camera_info->GetMessage()->binning_y(0); - - _impl_camera_info->GetMessage()->roi().x_offset(x_offset); - _impl_camera_info->GetMessage()->roi().y_offset(y_offset); - _impl_camera_info->GetMessage()->roi().height(height); - _impl_camera_info->GetMessage()->roi().width(width); - _impl_camera_info->GetMessage()->roi().do_rectify(do_rectify); + _impl_camera_info->GetMessage()->height = height; + _impl_camera_info->GetMessage()->width = width; + _impl_camera_info->GetMessage()->distortion_model = "plumb_bob"; + _impl_camera_info->GetMessage()->d = { 0.0, 0.0, 0.0, 0.0, 0.0 }; + _impl_camera_info->GetMessage()->k = {fx, 0.0, cx, 0.0, fy, cy, 0.0, 0.0, 1.0}; + _impl_camera_info->GetMessage()->r = { 1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0 }; + _impl_camera_info->GetMessage()->p = {fx, 0.0, cx, 0.0, 0.0, fy, cy, 0.0, 0.0, 0.0, 1.0, 0.0}; + _impl_camera_info->GetMessage()->binning_x = 0; + _impl_camera_info->GetMessage()->binning_y = 0; + + _impl_camera_info->GetMessage()->roi.x_offset = x_offset; + _impl_camera_info->GetMessage()->roi.y_offset = y_offset; + _impl_camera_info->GetMessage()->roi.height = height; + _impl_camera_info->GetMessage()->roi.width = width; + _impl_camera_info->GetMessage()->roi.do_rectify = do_rectify; return true; } bool CarlaCameraPublisher::WriteImage(int32_t seconds, uint32_t nanoseconds, uint32_t height, uint32_t width, std::vector data) { - _impl_image->GetMessage()->header().stamp().sec(seconds); - _impl_image->GetMessage()->header().stamp().nanosec(nanoseconds); - _impl_image->GetMessage()->header().frame_id(this->GetFrameId()); + _impl_image->GetMessage()->header.stamp.sec = seconds; + _impl_image->GetMessage()->header.stamp.nanosec = nanoseconds; + _impl_image->GetMessage()->header.frame_id = this->GetFrameId(); - _impl_image->GetMessage()->width(width); - _impl_image->GetMessage()->height(height); - _impl_image->GetMessage()->encoding(this->GetEncoding()); - _impl_image->GetMessage()->is_bigendian(0); - _impl_image->GetMessage()->step(width * this->GetChannels() * sizeof(uint8_t)); + _impl_image->GetMessage()->width = width; + _impl_image->GetMessage()->height = height; + _impl_image->GetMessage()->encoding = this->GetEncoding(); + _impl_image->GetMessage()->is_bigendian = 0; + _impl_image->GetMessage()->step = width * this->GetChannels() * sizeof(uint8_t); - _impl_image->GetMessage()->data(std::move(data)); // https://github.com/eProsima/Fast-DDS/issues/2330 + _impl_image->GetMessage()->data = std::move(data); return true; } diff --git a/LibCarla/source/carla/ros2/publishers/CarlaCameraPublisher.h b/LibCarla/source/carla/ros2/publishers/CarlaCameraPublisher.h index 523b152a5c7..e3870c34443 100644 --- a/LibCarla/source/carla/ros2/publishers/CarlaCameraPublisher.h +++ b/LibCarla/source/carla/ros2/publishers/CarlaCameraPublisher.h @@ -10,10 +10,8 @@ #include "carla/ros2/publishers/BasePublisher.h" #include "carla/ros2/publishers/PublisherImpl.h" -#include "carla/ros2/types/Image.h" -#include "carla/ros2/types/ImagePubSubTypes.h" -#include "carla/ros2/types/CameraInfo.h" -#include "carla/ros2/types/CameraInfoPubSubTypes.h" +#include "carla/ros2/types/msg/Image.h" +#include "carla/ros2/types/msg/CameraInfo.h" namespace carla { namespace ros2 { @@ -21,21 +19,23 @@ namespace ros2 { class CarlaCameraPublisher : public BasePublisher { public: struct ImageMsgTraits { - using msg_type = sensor_msgs::msg::Image; - using msg_pubsub_type = sensor_msgs::msg::ImagePubSubType; + using msg_type = msg::Image; }; struct CameraInfoMsgTraits { - using msg_type = sensor_msgs::msg::CameraInfo; - using msg_pubsub_type = sensor_msgs::msg::CameraInfoPubSubType; + using msg_type = msg::CameraInfo; }; CarlaCameraPublisher(std::string base_topic_name, std::string frame_id) : BasePublisher(base_topic_name, frame_id), _impl_image(std::make_shared>()), _impl_camera_info(std::make_shared>()) { - _impl_image->Init(GetBaseTopicName() + "/image"); - _impl_camera_info->Init(GetBaseTopicName() + "/camera_info"); + if (!_impl_image->Init(GetBaseTopicName() + "/image")) { + log_warning("CarlaCameraPublisher: Init failed for topic: ", GetBaseTopicName(), "/image"); + } + if (!_impl_camera_info->Init(GetBaseTopicName() + "/camera_info")) { + log_warning("CarlaCameraPublisher: Init failed for topic: ", GetBaseTopicName(), "/camera_info"); + } } virtual uint8_t GetChannels() = 0; diff --git a/LibCarla/source/carla/ros2/publishers/CarlaClockPublisher.cpp b/LibCarla/source/carla/ros2/publishers/CarlaClockPublisher.cpp index 2f287c25776..dbb42be55f7 100644 --- a/LibCarla/source/carla/ros2/publishers/CarlaClockPublisher.cpp +++ b/LibCarla/source/carla/ros2/publishers/CarlaClockPublisher.cpp @@ -8,8 +8,8 @@ namespace carla { namespace ros2 { bool CarlaClockPublisher::Write(int32_t seconds, uint32_t nanoseconds) { - _impl->GetMessage()->clock().sec(seconds); - _impl->GetMessage()->clock().nanosec(nanoseconds); + _impl->GetMessage()->clock.sec = seconds; + _impl->GetMessage()->clock.nanosec = nanoseconds; return true; } diff --git a/LibCarla/source/carla/ros2/publishers/CarlaClockPublisher.h b/LibCarla/source/carla/ros2/publishers/CarlaClockPublisher.h index 85dca7e225f..731e584a524 100644 --- a/LibCarla/source/carla/ros2/publishers/CarlaClockPublisher.h +++ b/LibCarla/source/carla/ros2/publishers/CarlaClockPublisher.h @@ -9,8 +9,7 @@ #include "carla/ros2/publishers/BasePublisher.h" #include "carla/ros2/publishers/PublisherImpl.h" -#include "carla/ros2/types/Clock.h" -#include "carla/ros2/types/ClockPubSubTypes.h" +#include "carla/ros2/types/msg/Clock.h" namespace carla { namespace ros2 { @@ -18,14 +17,15 @@ namespace ros2 { class CarlaClockPublisher : public BasePublisher { public: struct ClockMsgTraits { - using msg_type = rosgraph::msg::Clock; - using msg_pubsub_type = rosgraph::msg::ClockPubSubType; + using msg_type = msg::Clock; }; CarlaClockPublisher() : BasePublisher("rt/clock"), _impl(std::make_shared>()) { - _impl->Init(GetBaseTopicName()); + if (!_impl->Init(GetBaseTopicName())) { + log_warning("CarlaClockPublisher: Init failed for topic: ", GetBaseTopicName()); + } } bool Publish() { diff --git a/LibCarla/source/carla/ros2/publishers/CarlaCollisionPublisher.cpp b/LibCarla/source/carla/ros2/publishers/CarlaCollisionPublisher.cpp index c5879c9d7ed..f75341012ba 100644 --- a/LibCarla/source/carla/ros2/publishers/CarlaCollisionPublisher.cpp +++ b/LibCarla/source/carla/ros2/publishers/CarlaCollisionPublisher.cpp @@ -9,15 +9,15 @@ namespace ros2 { bool CarlaCollisionPublisher::Write(int32_t seconds, uint32_t nanoseconds, uint32_t actor_id, geom::Vector3D impulse) { - _impl->GetMessage()->header().stamp().sec(seconds); - _impl->GetMessage()->header().stamp().nanosec(nanoseconds); - _impl->GetMessage()->header().frame_id(GetFrameId()); + _impl->GetMessage()->header.stamp.sec = seconds; + _impl->GetMessage()->header.stamp.nanosec = nanoseconds; + _impl->GetMessage()->header.frame_id = GetFrameId(); - _impl->GetMessage()->other_actor_id(actor_id); + _impl->GetMessage()->other_actor_id = actor_id; - _impl->GetMessage()->normal_impulse().x(impulse.x); - _impl->GetMessage()->normal_impulse().y(impulse.y); - _impl->GetMessage()->normal_impulse().z(impulse.z); + _impl->GetMessage()->normal_impulse.x = impulse.x; + _impl->GetMessage()->normal_impulse.y = impulse.y; + _impl->GetMessage()->normal_impulse.z = impulse.z; return true; } diff --git a/LibCarla/source/carla/ros2/publishers/CarlaCollisionPublisher.h b/LibCarla/source/carla/ros2/publishers/CarlaCollisionPublisher.h index 081eda3dfe4..53d2d8649d0 100644 --- a/LibCarla/source/carla/ros2/publishers/CarlaCollisionPublisher.h +++ b/LibCarla/source/carla/ros2/publishers/CarlaCollisionPublisher.h @@ -11,8 +11,7 @@ #include "carla/ros2/publishers/BasePublisher.h" #include "carla/ros2/publishers/PublisherImpl.h" -#include "carla/ros2/types/CarlaCollisionEvent.h" -#include "carla/ros2/types/CarlaCollisionEventPubSubTypes.h" +#include "carla/ros2/types/msg/CarlaCollisionEvent.h" namespace carla { namespace ros2 { @@ -20,14 +19,15 @@ namespace ros2 { class CarlaCollisionPublisher : public BasePublisher { public: struct CollisionMsgTraits { - using msg_type = carla_msgs::msg::CarlaCollisionEvent; - using msg_pubsub_type = carla_msgs::msg::CarlaCollisionEventPubSubType; + using msg_type = msg::CarlaCollisionEvent; }; CarlaCollisionPublisher(std::string base_topic_name, std::string frame_id) : BasePublisher(base_topic_name, frame_id), _impl(std::make_shared>()) { - _impl->Init(this->GetBaseTopicName()); + if (!_impl->Init(this->GetBaseTopicName())) { + log_warning("CarlaCollisionPublisher: Init failed for topic: ", this->GetBaseTopicName()); + } } bool Publish() { diff --git a/LibCarla/source/carla/ros2/publishers/CarlaDVSPublisher.cpp b/LibCarla/source/carla/ros2/publishers/CarlaDVSPublisher.cpp index d47d624dc1c..ed8e11911b6 100644 --- a/LibCarla/source/carla/ros2/publishers/CarlaDVSPublisher.cpp +++ b/LibCarla/source/carla/ros2/publishers/CarlaDVSPublisher.cpp @@ -13,28 +13,31 @@ size_t CarlaDVSPointCloudPublisher::GetPointSize() { return sizeof(sensor::data::DVSEvent); } -std::vector CarlaDVSPointCloudPublisher::GetFields() { - - sensor_msgs::msg::PointField descriptor1; - descriptor1.name("x"); - descriptor1.offset(0); - descriptor1.datatype(sensor_msgs::msg::PointField__UINT16); - descriptor1.count(1); - sensor_msgs::msg::PointField descriptor2; - descriptor2.name("y"); - descriptor2.offset(2); - descriptor2.datatype(sensor_msgs::msg::PointField__UINT16); - descriptor2.count(1); - sensor_msgs::msg::PointField descriptor3; - descriptor3.name("t"); - descriptor3.offset(4); - descriptor3.datatype(sensor_msgs::msg::PointField__FLOAT64); - descriptor3.count(1); - sensor_msgs::msg::PointField descriptor4; - descriptor3.name("pol"); - descriptor3.offset(12); - descriptor3.datatype(sensor_msgs::msg::PointField__INT8); - descriptor3.count(1); +std::vector CarlaDVSPointCloudPublisher::GetFields() { + + msg::PointField descriptor1; + descriptor1.name = "x"; + descriptor1.offset = 0; + descriptor1.datatype = msg::PointField::UINT16; + descriptor1.count = 1; + + msg::PointField descriptor2; + descriptor2.name = "y"; + descriptor2.offset = 2; + descriptor2.datatype = msg::PointField::UINT16; + descriptor2.count = 1; + + msg::PointField descriptor3; + descriptor3.name = "t"; + descriptor3.offset = 4; + descriptor3.datatype = msg::PointField::FLOAT64; + descriptor3.count = 1; + + msg::PointField descriptor4; + descriptor4.name = "pol"; + descriptor4.offset = 12; + descriptor4.datatype = msg::PointField::INT8; + descriptor4.count = 1; return {descriptor1, descriptor2, descriptor3, descriptor4}; } diff --git a/LibCarla/source/carla/ros2/publishers/CarlaDVSPublisher.h b/LibCarla/source/carla/ros2/publishers/CarlaDVSPublisher.h index 0242ac96d6c..f4e6f5b8261 100644 --- a/LibCarla/source/carla/ros2/publishers/CarlaDVSPublisher.h +++ b/LibCarla/source/carla/ros2/publishers/CarlaDVSPublisher.h @@ -36,7 +36,7 @@ namespace ros2 { private: size_t GetPointSize() override; - std::vector GetFields() override; + std::vector GetFields() override; std::vector ComputePointCloud(uint32_t height, uint32_t width, uint8_t *data) override; }; diff --git a/LibCarla/source/carla/ros2/publishers/CarlaGNSSPublisher.cpp b/LibCarla/source/carla/ros2/publishers/CarlaGNSSPublisher.cpp index bb79bb56fa8..7309d6c609b 100644 --- a/LibCarla/source/carla/ros2/publishers/CarlaGNSSPublisher.cpp +++ b/LibCarla/source/carla/ros2/publishers/CarlaGNSSPublisher.cpp @@ -9,13 +9,13 @@ namespace ros2 { bool CarlaGNSSPublisher::Write(int32_t seconds, uint32_t nanoseconds, const geom::GeoLocation data) { - _impl->GetMessage()->header().stamp().sec(seconds); - _impl->GetMessage()->header().stamp().nanosec(nanoseconds); - _impl->GetMessage()->header().frame_id(GetFrameId()); + _impl->GetMessage()->header.stamp.sec = seconds; + _impl->GetMessage()->header.stamp.nanosec = nanoseconds; + _impl->GetMessage()->header.frame_id = GetFrameId(); - _impl->GetMessage()->latitude(data.latitude); - _impl->GetMessage()->longitude(data.longitude); - _impl->GetMessage()->altitude(data.altitude); + _impl->GetMessage()->latitude = data.latitude; + _impl->GetMessage()->longitude = data.longitude; + _impl->GetMessage()->altitude = data.altitude; return true; } diff --git a/LibCarla/source/carla/ros2/publishers/CarlaGNSSPublisher.h b/LibCarla/source/carla/ros2/publishers/CarlaGNSSPublisher.h index abcc0217ac6..4e5812739d3 100644 --- a/LibCarla/source/carla/ros2/publishers/CarlaGNSSPublisher.h +++ b/LibCarla/source/carla/ros2/publishers/CarlaGNSSPublisher.h @@ -11,8 +11,7 @@ #include "carla/ros2/publishers/BasePublisher.h" #include "carla/ros2/publishers/PublisherImpl.h" -#include "carla/ros2/types/NavSatFix.h" -#include "carla/ros2/types/NavSatFixPubSubTypes.h" +#include "carla/ros2/types/msg/NavSatFix.h" namespace carla { namespace ros2 { @@ -20,14 +19,15 @@ namespace ros2 { class CarlaGNSSPublisher : public BasePublisher { public: struct GnssMsgTraits { - using msg_type = sensor_msgs::msg::NavSatFix; - using msg_pubsub_type = sensor_msgs::msg::NavSatFixPubSubType; + using msg_type = msg::NavSatFix; }; CarlaGNSSPublisher(std::string base_topic_name, std::string frame_id): BasePublisher(base_topic_name, frame_id), _impl(std::make_shared>()) { - _impl->Init(this->GetBaseTopicName()); + if (!_impl->Init(this->GetBaseTopicName())) { + log_warning("CarlaGNSSPublisher: Init failed for topic: ", this->GetBaseTopicName()); + } } bool Publish() { diff --git a/LibCarla/source/carla/ros2/publishers/CarlaIMUPublisher.cpp b/LibCarla/source/carla/ros2/publishers/CarlaIMUPublisher.cpp index 3a2afa170b7..cf0d0a3cbca 100644 --- a/LibCarla/source/carla/ros2/publishers/CarlaIMUPublisher.cpp +++ b/LibCarla/source/carla/ros2/publishers/CarlaIMUPublisher.cpp @@ -9,17 +9,17 @@ namespace ros2 { bool CarlaIMUPublisher::Write(int32_t seconds, uint32_t nanoseconds, geom::Vector3D accelerometer, geom::Vector3D gyroscope, float compass) { - _impl->GetMessage()->header().stamp().sec(seconds); - _impl->GetMessage()->header().stamp().nanosec(nanoseconds); - _impl->GetMessage()->header().frame_id(GetFrameId()); + _impl->GetMessage()->header.stamp.sec = seconds; + _impl->GetMessage()->header.stamp.nanosec = nanoseconds; + _impl->GetMessage()->header.frame_id = GetFrameId(); - _impl->GetMessage()->linear_acceleration().x(accelerometer.x); - _impl->GetMessage()->linear_acceleration().y(-accelerometer.y); - _impl->GetMessage()->linear_acceleration().z(accelerometer.z); + _impl->GetMessage()->linear_acceleration.x = accelerometer.x; + _impl->GetMessage()->linear_acceleration.y = -accelerometer.y; + _impl->GetMessage()->linear_acceleration.z = accelerometer.z; - _impl->GetMessage()->angular_velocity().x(-gyroscope.x); - _impl->GetMessage()->angular_velocity().y(gyroscope.y); - _impl->GetMessage()->angular_velocity().z(-gyroscope.z); + _impl->GetMessage()->angular_velocity.x = -gyroscope.x; + _impl->GetMessage()->angular_velocity.y = gyroscope.y; + _impl->GetMessage()->angular_velocity.z = -gyroscope.z; const float rx = 0.0f; // pitch const float ry = (float(M_PI_2) / 2.0f) - compass; // yaw @@ -37,10 +37,10 @@ bool CarlaIMUPublisher::Write(int32_t seconds, uint32_t nanoseconds, geom::Vecto const float y = cr * sp * cy + sr * cp * sy; const float z = cr * cp * sy - sr * sp * cy; - _impl->GetMessage()->orientation().w(w); - _impl->GetMessage()->orientation().x(x); - _impl->GetMessage()->orientation().y(y); - _impl->GetMessage()->orientation().z(z); + _impl->GetMessage()->orientation.w = w; + _impl->GetMessage()->orientation.x = x; + _impl->GetMessage()->orientation.y = y; + _impl->GetMessage()->orientation.z = z; return true; } diff --git a/LibCarla/source/carla/ros2/publishers/CarlaIMUPublisher.h b/LibCarla/source/carla/ros2/publishers/CarlaIMUPublisher.h index e9d70472ed3..082a096679b 100644 --- a/LibCarla/source/carla/ros2/publishers/CarlaIMUPublisher.h +++ b/LibCarla/source/carla/ros2/publishers/CarlaIMUPublisher.h @@ -11,8 +11,7 @@ #include "carla/ros2/publishers/BasePublisher.h" #include "carla/ros2/publishers/PublisherImpl.h" -#include "carla/ros2/types/Imu.h" -#include "carla/ros2/types/ImuPubSubTypes.h" +#include "carla/ros2/types/msg/Imu.h" namespace carla { namespace ros2 { @@ -20,14 +19,15 @@ namespace ros2 { class CarlaIMUPublisher : public BasePublisher { public: struct ImuMsgTraits { - using msg_type = sensor_msgs::msg::Imu; - using msg_pubsub_type = sensor_msgs::msg::ImuPubSubType; + using msg_type = msg::Imu; }; CarlaIMUPublisher(std::string base_topic_name, std::string frame_id) : BasePublisher(base_topic_name, frame_id), _impl(std::make_shared>()) { - _impl->Init(this->GetBaseTopicName()); + if (!_impl->Init(this->GetBaseTopicName())) { + log_warning("CarlaIMUPublisher: Init failed for topic: ", this->GetBaseTopicName()); + } } bool Publish() { diff --git a/LibCarla/source/carla/ros2/publishers/CarlaLidarPublisher.cpp b/LibCarla/source/carla/ros2/publishers/CarlaLidarPublisher.cpp index d080de4f2e5..c46665cb970 100644 --- a/LibCarla/source/carla/ros2/publishers/CarlaLidarPublisher.cpp +++ b/LibCarla/source/carla/ros2/publishers/CarlaLidarPublisher.cpp @@ -13,31 +13,31 @@ size_t CarlaLidarPublisher::GetPointSize() { return sizeof(sensor::data::LidarDetection); } -std::vector CarlaLidarPublisher::GetFields() { - - sensor_msgs::msg::PointField descriptor1; - descriptor1.name("x"); - descriptor1.offset(0); - descriptor1.datatype(sensor_msgs::msg::PointField__FLOAT32); - descriptor1.count(1); - - sensor_msgs::msg::PointField descriptor2; - descriptor2.name("y"); - descriptor2.offset(4); - descriptor2.datatype(sensor_msgs::msg::PointField__FLOAT32); - descriptor2.count(1); - - sensor_msgs::msg::PointField descriptor3; - descriptor3.name("z"); - descriptor3.offset(8); - descriptor3.datatype(sensor_msgs::msg::PointField__FLOAT32); - descriptor3.count(1); - - sensor_msgs::msg::PointField descriptor4; - descriptor4.name("intensity"); - descriptor4.offset(12); - descriptor4.datatype(sensor_msgs::msg::PointField__FLOAT32); - descriptor4.count(1); +std::vector CarlaLidarPublisher::GetFields() { + + msg::PointField descriptor1; + descriptor1.name = "x"; + descriptor1.offset = 0; + descriptor1.datatype = msg::PointField::FLOAT32; + descriptor1.count = 1; + + msg::PointField descriptor2; + descriptor2.name = "y"; + descriptor2.offset = 4; + descriptor2.datatype = msg::PointField::FLOAT32; + descriptor2.count = 1; + + msg::PointField descriptor3; + descriptor3.name = "z"; + descriptor3.offset = 8; + descriptor3.datatype = msg::PointField::FLOAT32; + descriptor3.count = 1; + + msg::PointField descriptor4; + descriptor4.name = "intensity"; + descriptor4.offset = 12; + descriptor4.datatype = msg::PointField::FLOAT32; + descriptor4.count = 1; return {descriptor1, descriptor2, descriptor3, descriptor4}; } diff --git a/LibCarla/source/carla/ros2/publishers/CarlaLidarPublisher.h b/LibCarla/source/carla/ros2/publishers/CarlaLidarPublisher.h index e35d59cc550..bc13a5141dc 100644 --- a/LibCarla/source/carla/ros2/publishers/CarlaLidarPublisher.h +++ b/LibCarla/source/carla/ros2/publishers/CarlaLidarPublisher.h @@ -18,7 +18,7 @@ namespace ros2 { private: size_t GetPointSize() override; - std::vector GetFields() override; + std::vector GetFields() override; std::vector ComputePointCloud(uint32_t height, uint32_t width, uint8_t *data) override; }; diff --git a/LibCarla/source/carla/ros2/publishers/CarlaPointCloudPublisher.cpp b/LibCarla/source/carla/ros2/publishers/CarlaPointCloudPublisher.cpp index 7801657ea9c..cf46987737a 100644 --- a/LibCarla/source/carla/ros2/publishers/CarlaPointCloudPublisher.cpp +++ b/LibCarla/source/carla/ros2/publishers/CarlaPointCloudPublisher.cpp @@ -9,21 +9,21 @@ namespace ros2 { bool CarlaPointCloudPublisher::WritePointCloud(int32_t seconds, uint32_t nanoseconds, uint32_t height, uint32_t width, std::vector data) { - _impl->GetMessage()->header().stamp().sec(seconds); - _impl->GetMessage()->header().stamp().nanosec(nanoseconds); - _impl->GetMessage()->header().frame_id(GetFrameId()); + _impl->GetMessage()->header.stamp.sec = seconds; + _impl->GetMessage()->header.stamp.nanosec = nanoseconds; + _impl->GetMessage()->header.frame_id = GetFrameId(); auto fields = GetFields(); const size_t point_size = GetPointSize(); - _impl->GetMessage()->width(width); - _impl->GetMessage()->height(height); - _impl->GetMessage()->is_bigendian(false); - _impl->GetMessage()->fields(fields); - _impl->GetMessage()->point_step(static_cast(point_size)); - _impl->GetMessage()->row_step(static_cast(width * point_size)); - _impl->GetMessage()->is_dense(false); // True if there are not invalid points - _impl->GetMessage()->data(std::move(data)); + _impl->GetMessage()->width = width; + _impl->GetMessage()->height = height; + _impl->GetMessage()->is_bigendian = false; + _impl->GetMessage()->fields = std::move(fields); + _impl->GetMessage()->point_step = static_cast(point_size); + _impl->GetMessage()->row_step = width * static_cast(point_size); + _impl->GetMessage()->is_dense = false; + _impl->GetMessage()->data = std::move(data); return true; } diff --git a/LibCarla/source/carla/ros2/publishers/CarlaPointCloudPublisher.h b/LibCarla/source/carla/ros2/publishers/CarlaPointCloudPublisher.h index 6148f727090..64cebb4f242 100644 --- a/LibCarla/source/carla/ros2/publishers/CarlaPointCloudPublisher.h +++ b/LibCarla/source/carla/ros2/publishers/CarlaPointCloudPublisher.h @@ -10,8 +10,7 @@ #include "carla/ros2/publishers/BasePublisher.h" #include "carla/ros2/publishers/PublisherImpl.h" -#include "carla/ros2/types/PointCloud2.h" -#include "carla/ros2/types/PointCloud2PubSubTypes.h" +#include "carla/ros2/types/msg/PointCloud2.h" namespace carla { namespace ros2 { @@ -19,14 +18,15 @@ namespace ros2 { class CarlaPointCloudPublisher : public BasePublisher { public: struct PointCloudMsgTraits { - using msg_type = sensor_msgs::msg::PointCloud2; - using msg_pubsub_type = sensor_msgs::msg::PointCloud2PubSubType; + using msg_type = msg::PointCloud2; }; CarlaPointCloudPublisher(std::string base_topic_name, std::string frame_id) : BasePublisher(base_topic_name, frame_id), _impl(std::make_shared>()) { - _impl->Init(GetBaseTopicName() + "/point_cloud"); + if (!_impl->Init(GetBaseTopicName() + "/point_cloud")) { + log_warning("CarlaPointCloudPublisher: Init failed for topic: ", GetBaseTopicName(), "/point_cloud"); + } } bool Publish() { @@ -40,7 +40,7 @@ namespace ros2 { private: virtual size_t GetPointSize() = 0; - virtual std::vector GetFields() = 0; + virtual std::vector GetFields() = 0; virtual std::vector ComputePointCloud(uint32_t height, uint32_t width, uint8_t *data) = 0; diff --git a/LibCarla/source/carla/ros2/publishers/CarlaRadarPublisher.cpp b/LibCarla/source/carla/ros2/publishers/CarlaRadarPublisher.cpp index aceb1f6648a..b848197f4b5 100644 --- a/LibCarla/source/carla/ros2/publishers/CarlaRadarPublisher.cpp +++ b/LibCarla/source/carla/ros2/publishers/CarlaRadarPublisher.cpp @@ -20,49 +20,49 @@ size_t CarlaRadarPublisher::GetPointSize() { return sizeof(RadarDetectionWithPosition); } -std::vector CarlaRadarPublisher::GetFields() { - - sensor_msgs::msg::PointField descriptor1; - descriptor1.name("x"); - descriptor1.offset(0); - descriptor1.datatype(sensor_msgs::msg::PointField__FLOAT32); - descriptor1.count(1); - - sensor_msgs::msg::PointField descriptor2; - descriptor2.name("y"); - descriptor2.offset(4); - descriptor2.datatype(sensor_msgs::msg::PointField__FLOAT32); - descriptor2.count(1); - - sensor_msgs::msg::PointField descriptor3; - descriptor3.name("z"); - descriptor3.offset(8); - descriptor3.datatype(sensor_msgs::msg::PointField__FLOAT32); - descriptor3.count(1); - - sensor_msgs::msg::PointField descriptor4; - descriptor4.name("velocity"); - descriptor4.offset(12); - descriptor4.datatype(sensor_msgs::msg::PointField__FLOAT32); - descriptor4.count(1); - - sensor_msgs::msg::PointField descriptor5; - descriptor5.name("azimuth"); - descriptor5.offset(16); - descriptor5.datatype(sensor_msgs::msg::PointField__FLOAT32); - descriptor5.count(1); - - sensor_msgs::msg::PointField descriptor6; - descriptor6.name("altitude"); - descriptor6.offset(20); - descriptor6.datatype(sensor_msgs::msg::PointField__FLOAT32); - descriptor6.count(1); - - sensor_msgs::msg::PointField descriptor7; - descriptor7.name("depth"); - descriptor7.offset(24); - descriptor7.datatype(sensor_msgs::msg::PointField__FLOAT32); - descriptor7.count(1); +std::vector CarlaRadarPublisher::GetFields() { + + msg::PointField descriptor1; + descriptor1.name = "x"; + descriptor1.offset = 0; + descriptor1.datatype = msg::PointField::FLOAT32; + descriptor1.count = 1; + + msg::PointField descriptor2; + descriptor2.name = "y"; + descriptor2.offset = 4; + descriptor2.datatype = msg::PointField::FLOAT32; + descriptor2.count = 1; + + msg::PointField descriptor3; + descriptor3.name = "z"; + descriptor3.offset = 8; + descriptor3.datatype = msg::PointField::FLOAT32; + descriptor3.count = 1; + + msg::PointField descriptor4; + descriptor4.name = "velocity"; + descriptor4.offset = 12; + descriptor4.datatype = msg::PointField::FLOAT32; + descriptor4.count = 1; + + msg::PointField descriptor5; + descriptor5.name = "azimuth"; + descriptor5.offset = 16; + descriptor5.datatype = msg::PointField::FLOAT32; + descriptor5.count = 1; + + msg::PointField descriptor6; + descriptor6.name = "altitude"; + descriptor6.offset = 20; + descriptor6.datatype = msg::PointField::FLOAT32; + descriptor6.count = 1; + + msg::PointField descriptor7; + descriptor7.name = "depth"; + descriptor7.offset = 24; + descriptor7.datatype = msg::PointField::FLOAT32; + descriptor7.count = 1; return {descriptor1, descriptor2, descriptor3, descriptor4, descriptor5, descriptor6, descriptor7}; } diff --git a/LibCarla/source/carla/ros2/publishers/CarlaRadarPublisher.h b/LibCarla/source/carla/ros2/publishers/CarlaRadarPublisher.h index a7fe60511b1..a8c90b05338 100644 --- a/LibCarla/source/carla/ros2/publishers/CarlaRadarPublisher.h +++ b/LibCarla/source/carla/ros2/publishers/CarlaRadarPublisher.h @@ -19,7 +19,7 @@ namespace ros2 { private: size_t GetPointSize() override; - std::vector GetFields() override; + std::vector GetFields() override; std::vector ComputePointCloud(uint32_t height, uint32_t width, uint8_t *data) override; }; diff --git a/LibCarla/source/carla/ros2/publishers/CarlaSemanticLidarPublisher.cpp b/LibCarla/source/carla/ros2/publishers/CarlaSemanticLidarPublisher.cpp index 8a87193d840..d26b0ef6314 100644 --- a/LibCarla/source/carla/ros2/publishers/CarlaSemanticLidarPublisher.cpp +++ b/LibCarla/source/carla/ros2/publishers/CarlaSemanticLidarPublisher.cpp @@ -13,43 +13,43 @@ size_t CarlaSemanticLidarPublisher::GetPointSize() { return sizeof(sensor::data::SemanticLidarDetection); } -std::vector CarlaSemanticLidarPublisher::GetFields() { - - sensor_msgs::msg::PointField descriptor1; - descriptor1.name("x"); - descriptor1.offset(0); - descriptor1.datatype(sensor_msgs::msg::PointField__FLOAT32); - descriptor1.count(1); - - sensor_msgs::msg::PointField descriptor2; - descriptor2.name("y"); - descriptor2.offset(4); - descriptor2.datatype(sensor_msgs::msg::PointField__FLOAT32); - descriptor2.count(1); - - sensor_msgs::msg::PointField descriptor3; - descriptor3.name("z"); - descriptor3.offset(8); - descriptor3.datatype(sensor_msgs::msg::PointField__FLOAT32); - descriptor3.count(1); - - sensor_msgs::msg::PointField descriptor4; - descriptor4.name("cos_inc_angle"); - descriptor4.offset(12); - descriptor4.datatype(sensor_msgs::msg::PointField__FLOAT32); - descriptor4.count(1); - - sensor_msgs::msg::PointField descriptor5; - descriptor5.name("object_idx"); - descriptor5.offset(16); - descriptor5.datatype(sensor_msgs::msg::PointField__UINT32); - descriptor5.count(1); - - sensor_msgs::msg::PointField descriptor6; - descriptor6.name("object_tag"); - descriptor6.offset(20); - descriptor6.datatype(sensor_msgs::msg::PointField__UINT32); - descriptor6.count(1); +std::vector CarlaSemanticLidarPublisher::GetFields() { + + msg::PointField descriptor1; + descriptor1.name = "x"; + descriptor1.offset = 0; + descriptor1.datatype = msg::PointField::FLOAT32; + descriptor1.count = 1; + + msg::PointField descriptor2; + descriptor2.name = "y"; + descriptor2.offset = 4; + descriptor2.datatype = msg::PointField::FLOAT32; + descriptor2.count = 1; + + msg::PointField descriptor3; + descriptor3.name = "z"; + descriptor3.offset = 8; + descriptor3.datatype = msg::PointField::FLOAT32; + descriptor3.count = 1; + + msg::PointField descriptor4; + descriptor4.name = "cos_inc_angle"; + descriptor4.offset = 12; + descriptor4.datatype = msg::PointField::FLOAT32; + descriptor4.count = 1; + + msg::PointField descriptor5; + descriptor5.name = "object_idx"; + descriptor5.offset = 16; + descriptor5.datatype = msg::PointField::UINT32; + descriptor5.count = 1; + + msg::PointField descriptor6; + descriptor6.name = "object_tag"; + descriptor6.offset = 20; + descriptor6.datatype = msg::PointField::UINT32; + descriptor6.count = 1; return {descriptor1, descriptor2, descriptor3, descriptor4, descriptor5, descriptor6}; } diff --git a/LibCarla/source/carla/ros2/publishers/CarlaSemanticLidarPublisher.h b/LibCarla/source/carla/ros2/publishers/CarlaSemanticLidarPublisher.h index a76e57c01fb..57252de7db7 100644 --- a/LibCarla/source/carla/ros2/publishers/CarlaSemanticLidarPublisher.h +++ b/LibCarla/source/carla/ros2/publishers/CarlaSemanticLidarPublisher.h @@ -19,7 +19,7 @@ namespace ros2 { private: size_t GetPointSize() override; - std::vector GetFields() override; + std::vector GetFields() override; std::vector ComputePointCloud(uint32_t height, uint32_t width, uint8_t *data) override; }; diff --git a/LibCarla/source/carla/ros2/publishers/CarlaTransformPublisher.cpp b/LibCarla/source/carla/ros2/publishers/CarlaTransformPublisher.cpp index 10e3c985d55..3358f50bd08 100644 --- a/LibCarla/source/carla/ros2/publishers/CarlaTransformPublisher.cpp +++ b/LibCarla/source/carla/ros2/publishers/CarlaTransformPublisher.cpp @@ -9,7 +9,7 @@ namespace ros2 { constexpr double EPSILON = 1e-4; -geometry_msgs::msg::Transform CarlaTransformPublisher::ComputeTransform(std::string frame_id, geom::Transform transform) { +msg::Transform CarlaTransformPublisher::ComputeTransform(std::string frame_id, geom::Transform transform) { // Avoid recomputing the transform if it hasn't changed. // This is common for static sensors that are typically attached to other actors. @@ -49,35 +49,34 @@ geometry_msgs::msg::Transform CarlaTransformPublisher::ComputeTransform(std::str const float cy = cosf(ry * 0.5f); const float sy = sinf(ry * 0.5f); - geometry_msgs::msg::Transform tf; + msg::Transform tf; - tf.translation().x(tx); - tf.translation().y(ty); - tf.translation().z(tz); + tf.translation.x = tx; + tf.translation.y = ty; + tf.translation.z = tz; - tf.rotation().w(cr * cp * cy + sr * sp * sy); - tf.rotation().x(sr * cp * cy - cr * sp * sy); - tf.rotation().y(cr * sp * cy + sr * cp * sy); - tf.rotation().z(cr * cp * sy - sr * sp * cy); + tf.rotation.w = cr * cp * cy + sr * sp * sy; + tf.rotation.x = sr * cp * cy - cr * sp * sy; + tf.rotation.y = cr * sp * cy + sr * cp * sy; + tf.rotation.z = cr * cp * sy - sr * sp * cy; return tf; } bool CarlaTransformPublisher::Write(int32_t seconds, uint32_t nanoseconds, std::string frame_id, std::string child_frame_id, geom::Transform transform) { + msg::TransformStamped ts; - geometry_msgs::msg::TransformStamped ts; - - ts.header().stamp().sec(seconds); - ts.header().stamp().nanosec(nanoseconds); - ts.header().frame_id(frame_id); + ts.header.stamp.sec = seconds; + ts.header.stamp.nanosec = nanoseconds; + ts.header.frame_id = frame_id; auto tf = ComputeTransform(child_frame_id, transform); - ts.transform(tf); + ts.transform = tf; - ts.child_frame_id(child_frame_id); + ts.child_frame_id = child_frame_id; - _impl->GetMessage()->transforms({ts}); + _impl->GetMessage()->transforms = {ts}; // Update last transform information _last_transforms.insert({child_frame_id, {transform, tf}}); diff --git a/LibCarla/source/carla/ros2/publishers/CarlaTransformPublisher.h b/LibCarla/source/carla/ros2/publishers/CarlaTransformPublisher.h index 82bbe9125c8..d3f3749d337 100644 --- a/LibCarla/source/carla/ros2/publishers/CarlaTransformPublisher.h +++ b/LibCarla/source/carla/ros2/publishers/CarlaTransformPublisher.h @@ -12,8 +12,8 @@ #include "carla/ros2/publishers/BasePublisher.h" #include "carla/ros2/publishers/PublisherImpl.h" -#include "carla/ros2/types/TFMessage.h" -#include "carla/ros2/types/TFMessagePubSubTypes.h" +#include "carla/ros2/types/msg/TFMessage.h" +#include "carla/ros2/types/msg/Transform.h" namespace carla { namespace ros2 { @@ -21,14 +21,15 @@ namespace ros2 { class CarlaTransformPublisher : public BasePublisher { public: struct TransformMsgTraits { - using msg_type = tf2_msgs::msg::TFMessage; - using msg_pubsub_type = tf2_msgs::msg::TFMessagePubSubType; + using msg_type = msg::TFMessage; }; CarlaTransformPublisher() : BasePublisher("rt/tf"), _impl(std::make_shared>()) { - _impl->Init(GetBaseTopicName()); + if (!_impl->Init(GetBaseTopicName())) { + log_warning("CarlaTransformPublisher: Init failed for topic: ", GetBaseTopicName()); + } } bool Publish() { @@ -38,12 +39,12 @@ namespace ros2 { bool Write(int32_t seconds, uint32_t nanoseconds, std::string frame_id, std::string child_frame_id, geom::Transform transform); private: - geometry_msgs::msg::Transform ComputeTransform(std::string frame_id, geom::Transform current_transform); + msg::Transform ComputeTransform(std::string frame_id, geom::Transform current_transform); private: std::shared_ptr> _impl; - std::unordered_map> _last_transforms; + std::unordered_map> _last_transforms; }; } // namespace ros2 diff --git a/LibCarla/source/carla/ros2/subscribers/AckermannControlSubscriber.cpp b/LibCarla/source/carla/ros2/subscribers/AckermannControlSubscriber.cpp index 5a670c14a60..f6543071525 100644 --- a/LibCarla/source/carla/ros2/subscribers/AckermannControlSubscriber.cpp +++ b/LibCarla/source/carla/ros2/subscribers/AckermannControlSubscriber.cpp @@ -1,3 +1,7 @@ +// Copyright (c) 2025 Computer Vision Center (CVC) at the Universitat Autonoma de Barcelona (UAB). +// This work is licensed under the terms of the MIT license. +// For a copy, see . + #include "AckermannControlSubscriber.h" #include "carla/ros2/ROS2CallbackData.h" @@ -9,11 +13,11 @@ namespace ros2 { auto message = _impl->GetMessage(); AckermannControl control; - control.steer = message.drive().steering_angle(); - control.steer_speed = message.drive().steering_angle_velocity(); - control.speed = message.drive().speed(); - control.acceleration = message.drive().acceleration(); - control.jerk = message.drive().jerk(); + control.steer = message.drive.steering_angle; + control.steer_speed = message.drive.steering_angle_velocity; + control.speed = message.drive.speed; + control.acceleration = message.drive.acceleration; + control.jerk = message.drive.jerk; return control; } diff --git a/LibCarla/source/carla/ros2/subscribers/AckermannControlSubscriber.h b/LibCarla/source/carla/ros2/subscribers/AckermannControlSubscriber.h index 36749b4c098..6a306d28fd0 100644 --- a/LibCarla/source/carla/ros2/subscribers/AckermannControlSubscriber.h +++ b/LibCarla/source/carla/ros2/subscribers/AckermannControlSubscriber.h @@ -9,8 +9,7 @@ #include "BaseSubscriber.h" #include "SubscriberImpl.h" -#include "carla/ros2/types/AckermannDriveStamped.h" -#include "carla/ros2/types/AckermannDriveStampedPubSubTypes.h" +#include "carla/ros2/types/msg/AckermannDriveStamped.h" #include "carla/ros2/ROS2CallbackData.h" @@ -20,15 +19,17 @@ namespace ros2 { class AckermannControlSubscriber : public BaseSubscriber { public: struct AckermannMsgTraits { - using msg_type = ackermann_msgs::msg::AckermannDriveStamped; - using msg_pubsub_type = ackermann_msgs::msg::AckermannDriveStampedPubSubType; + using msg_type = msg::AckermannDriveStamped; }; AckermannControlSubscriber(void* vehicle, std::string base_topic_name, std::string frame_id) : BaseSubscriber(vehicle, base_topic_name, frame_id), _impl(std::make_shared>()) { - _impl->Init(this->GetBaseTopicName() + "/ackermann_control_cmd"); + if (!_impl->Init(this->GetBaseTopicName() + "/ackermann_control_cmd")) { + log_warning("AckermannControlSubscriber: Init failed for topic: ", + this->GetBaseTopicName() + "/ackermann_control_cmd"); + } } ROS2CallbackData GetMessage(); diff --git a/LibCarla/source/carla/ros2/subscribers/CarlaEgoVehicleControlSubscriber.cpp b/LibCarla/source/carla/ros2/subscribers/CarlaEgoVehicleControlSubscriber.cpp index d3d3e7b646b..e63cd675c46 100644 --- a/LibCarla/source/carla/ros2/subscribers/CarlaEgoVehicleControlSubscriber.cpp +++ b/LibCarla/source/carla/ros2/subscribers/CarlaEgoVehicleControlSubscriber.cpp @@ -1,3 +1,7 @@ +// Copyright (c) 2025 Computer Vision Center (CVC) at the Universitat Autonoma de Barcelona (UAB). +// This work is licensed under the terms of the MIT license. +// For a copy, see . + #include "CarlaEgoVehicleControlSubscriber.h" #include "carla/ros2/ROS2CallbackData.h" @@ -9,13 +13,13 @@ namespace ros2 { auto message = _impl->GetMessage(); VehicleControl control; - control.throttle = message.throttle(); - control.steer = message.steer(); - control.brake = message.brake(); - control.hand_brake = message.hand_brake(); - control.reverse = message.reverse(); - control.gear = message.gear(); - control.manual_gear_shift = message.manual_gear_shift(); + control.throttle = message.throttle; + control.steer = message.steer; + control.brake = message.brake; + control.hand_brake = message.hand_brake; + control.reverse = message.reverse; + control.gear = message.gear; + control.manual_gear_shift = message.manual_gear_shift; return control; } diff --git a/LibCarla/source/carla/ros2/subscribers/CarlaEgoVehicleControlSubscriber.h b/LibCarla/source/carla/ros2/subscribers/CarlaEgoVehicleControlSubscriber.h index 39f108fb30d..573db35e700 100644 --- a/LibCarla/source/carla/ros2/subscribers/CarlaEgoVehicleControlSubscriber.h +++ b/LibCarla/source/carla/ros2/subscribers/CarlaEgoVehicleControlSubscriber.h @@ -9,8 +9,7 @@ #include "BaseSubscriber.h" #include "SubscriberImpl.h" -#include "carla/ros2/types/CarlaEgoVehicleControl.h" -#include "carla/ros2/types/CarlaEgoVehicleControlPubSubTypes.h" +#include "carla/ros2/types/msg/CarlaEgoVehicleControl.h" #include "carla/ros2/ROS2CallbackData.h" @@ -20,15 +19,17 @@ namespace ros2 { class CarlaEgoVehicleControlSubscriber : public BaseSubscriber { public: struct ControlMsgTraits { - using msg_type = carla_msgs::msg::CarlaEgoVehicleControl; - using msg_pubsub_type = carla_msgs::msg::CarlaEgoVehicleControlPubSubType; + using msg_type = msg::CarlaEgoVehicleControl; }; CarlaEgoVehicleControlSubscriber(void* vehicle, std::string base_topic_name, std::string frame_id) : BaseSubscriber(vehicle, base_topic_name, frame_id), _impl(std::make_shared>()) { - _impl->Init(this->GetBaseTopicName() + "/vehicle_control_cmd"); + if (!_impl->Init(this->GetBaseTopicName() + "/vehicle_control_cmd")) { + log_warning("CarlaEgoVehicleControlSubscriber: Init failed for topic: ", + this->GetBaseTopicName() + "/vehicle_control_cmd"); + } } ROS2CallbackData GetMessage(); From 2a4a8db3def0d426be56f7c303cee1ebc931a4b1 Mon Sep 17 00:00:00 2001 From: Jesus Armando Anaya Date: Mon, 30 Mar 2026 02:12:49 -0700 Subject: [PATCH 4/4] feat(LibCarla/ros2): add CycloneDDS to middleware enum, factory, and tests Register CycloneDDS as a recognized middleware option in DDSMiddleware enum, DDSMiddlewareToString, DDSMiddlewareFromString, and GetAvailableMiddlewareString. Add CycloneDDS case to DDSMiddlewareFactory for IsMiddlewareAvailable, CreatePublisher, and CreateSubscriber, all guarded by CARLA_ROS2_DDS_CYCLONEDDS. Add stub CycloneDDSPublisherMiddleware and CycloneDDSSubscriberMiddleware that compile and satisfy the factory interface but return failure on all operations until the real implementation lands. Add 7 new unit tests for CycloneDDS enum parsing, availability checks, and factory routing when the middleware is not compiled in. --- .../source/carla/ros2/dds/DDSMiddleware.h | 14 +++- .../carla/ros2/dds/DDSMiddlewareFactory.h | 27 +++++++ .../CycloneDDSPublisherMiddleware.h | 45 ++++++++++++ .../CycloneDDSSubscriberMiddleware.h | 44 ++++++++++++ .../test/server/test_dds_middleware.cpp | 70 ++++++++++++++++--- 5 files changed, 189 insertions(+), 11 deletions(-) create mode 100644 LibCarla/source/carla/ros2/dds/cyclonedds/CycloneDDSPublisherMiddleware.h create mode 100644 LibCarla/source/carla/ros2/dds/cyclonedds/CycloneDDSSubscriberMiddleware.h diff --git a/LibCarla/source/carla/ros2/dds/DDSMiddleware.h b/LibCarla/source/carla/ros2/dds/DDSMiddleware.h index 20f8804c628..83811d7ad87 100644 --- a/LibCarla/source/carla/ros2/dds/DDSMiddleware.h +++ b/LibCarla/source/carla/ros2/dds/DDSMiddleware.h @@ -13,7 +13,8 @@ namespace ros2 { /// Passed to ROS2::Enable() to select the middleware at startup. /// Once set, the middleware cannot be changed without restarting. enum class DDSMiddleware { - FastDDS + FastDDS, + CycloneDDS }; /// Convert a DDSMiddleware enum value to a readable string. @@ -21,6 +22,8 @@ inline const char* DDSMiddlewareToString(DDSMiddleware middleware) { switch (middleware) { case DDSMiddleware::FastDDS: return "FastDDS"; + case DDSMiddleware::CycloneDDS: + return "CycloneDDS"; } return "Unknown"; } @@ -37,6 +40,9 @@ inline DDSMiddlewareParseResult DDSMiddlewareFromString(const std::string& name) if (name == "fastdds") { return {true, DDSMiddleware::FastDDS}; } + if (name == "cyclonedds") { + return {true, DDSMiddleware::CycloneDDS}; + } return {false, DDSMiddleware::FastDDS}; } @@ -45,6 +51,12 @@ inline std::string GetAvailableMiddlewareString() { std::string result; #if defined(CARLA_ROS2_DDS_FASTDDS) result += "FastDDS"; +#endif +#if defined(CARLA_ROS2_DDS_CYCLONEDDS) + if (!result.empty()) { + result += ", "; + } + result += "CycloneDDS"; #endif if (result.empty()) { result = "none"; diff --git a/LibCarla/source/carla/ros2/dds/DDSMiddlewareFactory.h b/LibCarla/source/carla/ros2/dds/DDSMiddlewareFactory.h index 51cc8f81143..36adc6e6570 100644 --- a/LibCarla/source/carla/ros2/dds/DDSMiddlewareFactory.h +++ b/LibCarla/source/carla/ros2/dds/DDSMiddlewareFactory.h @@ -17,6 +17,11 @@ # include "carla/ros2/dds/fastdds/FastDDSSubscriberMiddleware.h" #endif +#if defined(CARLA_ROS2_DDS_CYCLONEDDS) && !defined(CARLA_ROS2_DDS_TESTING) +# include "carla/ros2/dds/cyclonedds/CycloneDDSPublisherMiddleware.h" +# include "carla/ros2/dds/cyclonedds/CycloneDDSSubscriberMiddleware.h" +#endif + namespace carla { namespace ros2 { @@ -44,6 +49,12 @@ class DDSMiddlewareFactory { return true; #else return false; +#endif + case DDSMiddleware::CycloneDDS: +#if defined(CARLA_ROS2_DDS_CYCLONEDDS) + return true; +#else + return false; #endif } return false; @@ -83,6 +94,14 @@ class DDSMiddlewareFactory { #else log_error("DDSMiddlewareFactory: FastDDS not compiled in"); return nullptr; +#endif + case DDSMiddleware::CycloneDDS: +#if defined(CARLA_ROS2_DDS_CYCLONEDDS) && !defined(CARLA_ROS2_DDS_TESTING) + return std::unique_ptr( + new CycloneDDSPublisherMiddleware()); +#else + log_error("DDSMiddlewareFactory: CycloneDDS not compiled in"); + return nullptr; #endif } return nullptr; @@ -101,6 +120,14 @@ class DDSMiddlewareFactory { #else log_error("DDSMiddlewareFactory: FastDDS not compiled in"); return nullptr; +#endif + case DDSMiddleware::CycloneDDS: +#if defined(CARLA_ROS2_DDS_CYCLONEDDS) && !defined(CARLA_ROS2_DDS_TESTING) + return std::unique_ptr( + new CycloneDDSSubscriberMiddleware()); +#else + log_error("DDSMiddlewareFactory: CycloneDDS not compiled in"); + return nullptr; #endif } return nullptr; diff --git a/LibCarla/source/carla/ros2/dds/cyclonedds/CycloneDDSPublisherMiddleware.h b/LibCarla/source/carla/ros2/dds/cyclonedds/CycloneDDSPublisherMiddleware.h new file mode 100644 index 00000000000..383b937056d --- /dev/null +++ b/LibCarla/source/carla/ros2/dds/cyclonedds/CycloneDDSPublisherMiddleware.h @@ -0,0 +1,45 @@ +// Copyright (c) 2025 Computer Vision Center (CVC) at the Universitat Autonoma de Barcelona (UAB). +// This work is licensed under the terms of the MIT license. +// For a copy, see . + +#pragma once + +#include "carla/ros2/dds/IDDSPublisherMiddleware.h" +#include "carla/Logging.h" + +namespace carla { +namespace ros2 { + +/// CycloneDDS implementation of IDDSPublisherMiddleware (stub). +/// +/// This is a placeholder that compiles and satisfies the factory interface +/// but does not contain a real CycloneDDS implementation. All operations +/// log an error and return failure. The real implementation will replace +/// this file once CycloneDDS types and the C API are available. +/// +/// Parameterized on a traits type T that provides: +/// T::msg_type — middleware-neutral POD message struct +template +class CycloneDDSPublisherMiddleware : public IDDSPublisherMiddleware { + public: + bool Init(const std::string& topic_name) override { + log_error("CycloneDDSPublisherMiddleware: stub — not yet implemented " + "(topic '", topic_name, "')"); + return false; + } + + bool Publish(void* /*message_data*/) override { + return false; + } + + bool IsAlive() const override { + return false; + } + + std::string GetTopicName() const override { + return {}; + } +}; + +} // namespace ros2 +} // namespace carla diff --git a/LibCarla/source/carla/ros2/dds/cyclonedds/CycloneDDSSubscriberMiddleware.h b/LibCarla/source/carla/ros2/dds/cyclonedds/CycloneDDSSubscriberMiddleware.h new file mode 100644 index 00000000000..174ff99b8b5 --- /dev/null +++ b/LibCarla/source/carla/ros2/dds/cyclonedds/CycloneDDSSubscriberMiddleware.h @@ -0,0 +1,44 @@ +// Copyright (c) 2025 Computer Vision Center (CVC) at the Universitat Autonoma de Barcelona (UAB). +// This work is licensed under the terms of the MIT license. +// For a copy, see . + +#pragma once + +#include "carla/ros2/dds/IDDSSubscriberMiddleware.h" +#include "carla/Logging.h" + +namespace carla { +namespace ros2 { + +/// CycloneDDS implementation of IDDSSubscriberMiddleware (stub). +/// +/// This is a placeholder that compiles and satisfies the factory interface +/// but does not contain a real CycloneDDS implementation. All operations +/// log an error and return failure. The real implementation will replace +/// this file once CycloneDDS types and the C API are available. +/// +/// Parameterized on a traits type S that provides: +/// S::msg_type — middleware-neutral POD message struct +template +class CycloneDDSSubscriberMiddleware : public IDDSSubscriberMiddleware { + public: + bool Init( + const std::string& topic_name, + void* /*message_ptr*/, + bool* /*new_message_flag*/) override { + log_error("CycloneDDSSubscriberMiddleware: stub — not yet implemented " + "(topic '", topic_name, "')"); + return false; + } + + bool IsAlive() const override { + return false; + } + + std::string GetTopicName() const override { + return {}; + } +}; + +} // namespace ros2 +} // namespace carla diff --git a/LibCarla/source/test/server/test_dds_middleware.cpp b/LibCarla/source/test/server/test_dds_middleware.cpp index a4563b94c81..5ec7116292c 100644 --- a/LibCarla/source/test/server/test_dds_middleware.cpp +++ b/LibCarla/source/test/server/test_dds_middleware.cpp @@ -107,19 +107,26 @@ class DDSMiddlewareFactoryFixture : public ::testing::Test { // ========================================================================== TEST(dds_middleware_enum, values_exist) { - DDSMiddleware mw = DDSMiddleware::FastDDS; - EXPECT_EQ(static_cast(mw), 0); + DDSMiddleware mw_fast = DDSMiddleware::FastDDS; + EXPECT_EQ(static_cast(mw_fast), 0); + DDSMiddleware mw_cyclone = DDSMiddleware::CycloneDDS; + EXPECT_EQ(static_cast(mw_cyclone), 1); } TEST(dds_middleware_enum, switch_covers_all) { - DDSMiddleware mw = DDSMiddleware::FastDDS; - bool covered = false; - switch (mw) { - case DDSMiddleware::FastDDS: - covered = true; - break; + DDSMiddleware values[] = {DDSMiddleware::FastDDS, DDSMiddleware::CycloneDDS}; + for (auto mw : values) { + bool covered = false; + switch (mw) { + case DDSMiddleware::FastDDS: + covered = true; + break; + case DDSMiddleware::CycloneDDS: + covered = true; + break; + } + EXPECT_TRUE(covered); } - EXPECT_TRUE(covered); } // ========================================================================== @@ -140,6 +147,10 @@ TEST(dds_middleware_to_string, result_is_not_empty) { EXPECT_STRNE(result, ""); } +TEST(dds_middleware_to_string, cyclonedds_returns_correct_string) { + EXPECT_STREQ(DDSMiddlewareToString(DDSMiddleware::CycloneDDS), "CycloneDDS"); +} + // ========================================================================== // Group 3: dds_middleware_from_string (5 tests) // ========================================================================== @@ -151,10 +162,16 @@ TEST(dds_middleware_from_string, fastdds_lowercase_valid) { } TEST(dds_middleware_from_string, unknown_string_invalid) { - auto result = DDSMiddlewareFromString("cyclonedds"); + auto result = DDSMiddlewareFromString("unknowndds"); EXPECT_FALSE(result.valid); } +TEST(dds_middleware_from_string, cyclonedds_lowercase_valid) { + auto result = DDSMiddlewareFromString("cyclonedds"); + EXPECT_TRUE(result.valid); + EXPECT_EQ(result.middleware, DDSMiddleware::CycloneDDS); +} + TEST(dds_middleware_from_string, empty_string_invalid) { auto result = DDSMiddlewareFromString(""); EXPECT_FALSE(result.valid); @@ -184,6 +201,11 @@ TEST(dds_middleware_available, available_string_contains_fastdds) { EXPECT_NE(available.find("FastDDS"), std::string::npos); } +TEST(dds_middleware_available, cyclonedds_not_available_without_macro) { + EXPECT_FALSE( + DDSMiddlewareFactory::IsMiddlewareAvailable(DDSMiddleware::CycloneDDS)); +} + // ========================================================================== // Group 5: dds_middleware_type_name (4 tests) // ========================================================================== @@ -236,6 +258,34 @@ TEST_F(DDSMiddlewareFactoryFixture, factory_available_string) { EXPECT_NE(available.find("FastDDS"), std::string::npos); } +TEST_F(DDSMiddlewareFactoryFixture, set_and_get_cyclonedds) { + DDSMiddlewareFactory::SetMiddleware(DDSMiddleware::CycloneDDS); + EXPECT_EQ(DDSMiddlewareFactory::GetMiddleware(), DDSMiddleware::CycloneDDS); +} + +TEST_F(DDSMiddlewareFactoryFixture, resolve_unavailable_cyclonedds) { + auto resolution = + DDSMiddlewareFactory::ResolveMiddleware(DDSMiddleware::CycloneDDS); + EXPECT_FALSE(resolution.success); + EXPECT_EQ(resolution.middleware, DDSMiddleware::CycloneDDS); +} + +TEST_F(DDSMiddlewareFactoryFixture, create_publisher_cyclonedds_unavailable) { + DDSMiddlewareFactory::SetMiddleware(DDSMiddleware::CycloneDDS); + ::testing::internal::CaptureStderr(); + auto pub = DDSMiddlewareFactory::CreatePublisher(); + ::testing::internal::GetCapturedStderr(); + EXPECT_EQ(pub, nullptr); +} + +TEST_F(DDSMiddlewareFactoryFixture, create_subscriber_cyclonedds_unavailable) { + DDSMiddlewareFactory::SetMiddleware(DDSMiddleware::CycloneDDS); + ::testing::internal::CaptureStderr(); + auto sub = DDSMiddlewareFactory::CreateSubscriber(); + ::testing::internal::GetCapturedStderr(); + EXPECT_EQ(sub, nullptr); +} + // ========================================================================== // Group 7: dds_publisher_interface (5 tests) // ==========================================================================