Ros2 (#6862)

* Adding ROS2 manager

* Trying to compile fastDDS in windows, but have problems with dependencies

* Camera sensors connected to ROS2, dependencies disabled temporaly

* Including enum with sensors list

* adding GNSS sensor capture

* adding IMU sensor capture

* adding DVS sensor capture

* adding Lidar sensor capture

* adding SemanticLidar and Radar capture

* adding ObstacleDetector and some fixes

* modify cmakelists, failed to find libatomic

* fixed libatomic

* compile fast-dds with libc++

* fixed compliation fast dds with libcxx

* linked carla with fastdds

* Fixed Fast-DDS lib
Now its compiled to a intermediate lib with a bridge so libstdc++ can be used

* moved all ros2 deps to ros2 folder
renamed types from .cxx to .cpp

* Finally FastDDS compiled and working inside CARLA

* exposed publish function for testing

* fixed code style

* make ros2 optional

* make ros2 optional

* Added defines to compile ROS2 code

* ros image publisher working

* deleted some buffer copies

* Added more topics  and ROS2 types

* Adding sensor Transform as argument, and add function to send buffer or serialize and send buffer

* Removing empty buffer from DVS sensor

* fixed lidar publisher

* fixed lidar publisher

* Fix buffer of RGB in ROS2

* adding timestamp of each frame into ROS2 manager

* sending timestamps with frame to ROS2

* put apart timestamp from frame counter call

* adding BufferView to share buffers

* adding BufferView to share buffers

* adding ros_name attribute to all actors

* mapping ros_name for each actor

* ROS2 is now published in a different thread
Publishers now can be created on demand and be reused
Added subscriber for ego vehicle
Fixed build scripts

* forgot to add this fix

* add ros2 to windows scripts

* fixed default ros topic names

* fix topic name duplication

* Adding functions for enable/disable sensors publishing without listen to it

* Added Camera info and ROI types
RGB Camera now publishes in both topics

* move camera info immutable data to the constructor

* Publish transform for all topics
Create Camera DVS subtopics
Fixed bug with Lidar

* Added flip Y to semantic lidar

* Adding callbacks for subscribers from Unreal

* Adding camera info to ROS2 (resolution and FOV)

* Finished ROS2 naming from python

* Fixed bug with ros names

* Delete topics when deleting the sensors

* setting the rosname same as default no longer rewrites it

* ROS transform fix

* Added all camera topics with the image and camera info
Added vehicle control
Added clock publisher

* Change ros2 topic names for vehicle subscriber and  clock publisher

* Rename vehicle control ros2 topic name

* rename subscriber type to CarlaEgoVehicleControl

* Fix semantic lidar default ros name
Fix sizeof the semantic lidar data buffer

* Changed controller stored as string to pointer

* Back to previous version for default hero ros name
Removed debug prints

* Remvoe callback when ego publisher disconnects

* Serializing DVS data before sending to ROS

* dvs camera image and lidar

* DVS Pointcloud publishing

* Remove unnecessary fields

* Send local transforms to ros

* avoid transform recalculation if not needed

* Optical Flow Camera fixed

* Set fixed branch for Fast-DDS to avoid cmake version change issues

* Finally Working on package

* Fix style from FastDDS auto generated files

* Added ros2 to ubuntu in jenkins

* removed ros2 from jenkins, test

* restore ros2 in jenkins

* fix copy shareds, and removed server dependency from libcarla

* test installing the fastdds dependencies in jenkins

* move installing deps to separate stage

* removed install deps

* Fixing test_benchmark_streaming

* Fixed imu orientation and camera info data

* Fixing test_benchmark_streaming.cpp with BufferView

* Removing DEBUG_ONLY()

* publish collision sensor

* Fixing unit-tests with the new BufferView

* camera info is set once

* Fix echo camera info

* fix transform rotation

* Fixing ros_name attribute creation

* fixed camera info and region of interest publish

* fix IMU compass

* Forgot to add ros2 flag to jenkins package

---------

Co-authored-by: bernatx <bernatx@gmail.com>

CHANGELOG.md

@@ -3,6 +3,10 @@
   * Added build support for VS2022 and Ninja for LibCarla and osm2odr on Windows
   * Fixed bug causing the TM's unstuck logic to incorrectly remove the vehicles in some situations.
   * Fixed the extra data in Directx textures, so we need to copy row by row on Windows to remove extra bytes on images
+  * Added API functions to enable sensor data publishing for ROS2 without listen to it
+    * `Sensor.enable_for_ros()`
+    * `Sensor.disable_for_ros()`
+    * `Sensor.is_enabled_for_ros()`
   * Fixed vertices of big meshes (more than 65k vertices) in CarlaExporter
   * Fixed sensors to check for the stream to be ready (race condition)
   * Added empty actor

Jenkinsfile

@@ -44,7 +44,7 @@ pipeline
                             steps
                             {
                                 sh 'git update-index --skip-worktree Unreal/CarlaUE4/CarlaUE4.uproject'
-                                sh 'make setup ARGS="--python-version=3.7,2 --target-wheel-platform=manylinux_2_27_x86_64 --chrono"'
+                                sh 'make setup ARGS="--python-version=3.7,2 --target-wheel-platform=manylinux_2_27_x86_64 --chrono --ros2"'
                             }
                         }
                         stage('ubuntu build')
@@ -53,7 +53,7 @@ pipeline
                             {
                                 sh 'make LibCarla'
                                 sh 'make PythonAPI ARGS="--python-version=3.7,2 --target-wheel-platform=manylinux_2_27_x86_64"'
-                                sh 'make CarlaUE4Editor ARGS="--chrono"'
+                                sh 'make CarlaUE4Editor ARGS="--chrono --ros2"'
                                 sh 'make plugins'
                                 sh 'make examples'
                             }
@@ -94,7 +94,7 @@ pipeline
                         {
                             steps
                             {
-                                sh 'make package ARGS="--python-version=3.7,2 --target-wheel-platform=manylinux_2_27_x86_64 --chrono"'
+                                sh 'make package ARGS="--python-version=3.7,2 --target-wheel-platform=manylinux_2_27_x86_64 --chrono --ros2"'
                                 sh 'make package ARGS="--packages=AdditionalMaps,Town06_Opt,Town07_Opt,Town11,Town12,Town13,Town15 --target-archive=AdditionalMaps --clean-intermediate --python-version=3.7,2 --target-wheel-platform=manylinux_2_27_x86_64"'
                                 sh 'make examples ARGS="localhost 3654"'
                             }

LibCarla/cmake/CMakeLists.txt

@@ -25,11 +25,13 @@ elseif (CMAKE_BUILD_TYPE STREQUAL "Server")
   add_subdirectory("server")
 elseif (CMAKE_BUILD_TYPE STREQUAL "Pytorch")
 add_subdirectory("pytorch")
+elseif (CMAKE_BUILD_TYPE STREQUAL "ros2")
+add_subdirectory("fast_dds")
 else ()
   message(FATAL_ERROR "Unknown build type '${CMAKE_BUILD_TYPE}'")
 endif ()
 
 # GTest is not compiled on Windows.
-if ((LIBCARLA_BUILD_TEST) AND (NOT WIN32) AND (NOT (CMAKE_BUILD_TYPE STREQUAL "Pytorch")))
+if ((LIBCARLA_BUILD_TEST) AND (NOT WIN32) AND (NOT (CMAKE_BUILD_TYPE STREQUAL "Pytorch")) AND (NOT (CMAKE_BUILD_TYPE STREQUAL "ros2")))
   add_subdirectory("test")
 endif()

LibCarla/cmake/client/CMakeLists.txt

@@ -254,6 +254,12 @@ file(GLOB libcarla_carla_trafficmanager_sources
 set(libcarla_sources "${libcarla_sources};${libcarla_carla_trafficmanager_sources}")
 install(FILES ${libcarla_carla_trafficmanager_sources} DESTINATION include/carla/trafficmanager)
 
+file(GLOB libcarla_carla_ros2_sources
+    "${libcarla_source_path}/carla/ros2/*.cpp"
+    "${libcarla_source_path}/carla/ros2/*.h")
+set(libcarla_sources "${libcarla_sources};${libcarla_carla_ros2_sources}")
+install(FILES ${libcarla_carla_ros2_sources} DESTINATION include/carla/ros2)
+
 # ==============================================================================
 # Create targets for debug and release in the same build type.
 # ==============================================================================

LibCarla/cmake/fast_dds/CMakeLists.txt

@@ -0,0 +1,56 @@
+cmake_minimum_required(VERSION 3.5.1)
+project(libcarla_fastdds)
+
+# Install headers.
+
+file(GLOB libcarla_carla_fastdds_headers
+  "${libcarla_source_path}/carla/ros2/publishers/*.h"
+  "${libcarla_source_path}/carla/ros2/subscribers/*.h"
+  "${libcarla_source_path}/carla/ros2/listeners/*.h"
+  "${libcarla_source_path}/carla/ros2/types/*.h"
+  )
+install(FILES ${libcarla_carla_fastdds_headers} DESTINATION include/carla/ros2)
+
+file(GLOB fast_dds_dependencies "${FASTDDS_LIB_PATH}/*.so*")
+install(FILES ${fast_dds_dependencies} DESTINATION lib)
+
+
+file(GLOB libcarla_fastdds_sources
+  "${libcarla_source_path}/carla/ros2/publishers/*.cpp"
+  "${libcarla_source_path}/carla/ros2/subscribers/*.cpp"
+  "${libcarla_source_path}/carla/ros2/listeners/*.cpp"
+  "${libcarla_source_path}/carla/ros2/types/*.cpp")
+
+set(FASTDDS_CPP_STD_INCLUDES "/usr/include/c++/7")
+set(CMAKE_CXX_FLAGS_RELEASE "-O3 -fPIC -D_GLIBCXX_USE_CXX11_ABI=0 -I${FASTDDS_CPP_STD_INCLUDES}" CACHE STRING "" FORCE)
+
+# ==============================================================================
+# Create targets for debug and release in the same build type.
+# ==============================================================================
+
+if (LIBCARLA_BUILD_RELEASE)
+  add_library(carla_fastdds STATIC ${libcarla_fastdds_sources})
+
+  target_include_directories(carla_fastdds SYSTEM PRIVATE
+      "${BOOST_INCLUDE_PATH}"
+      "${RPCLIB_INCLUDE_PATH}")
+
+  target_include_directories(carla_fastdds PRIVATE "${FASTDDS_INCLUDE_PATH}")
+  target_include_directories(carla_fastdds PRIVATE "${libcarla_source_path}/carla/ros2")
+  target_link_libraries(carla_fastdds fastrtps fastcdr "${FAST_DDS_LIBRARIES}")
+  install(TARGETS carla_fastdds DESTINATION lib)
+  set_target_properties(carla_fastdds PROPERTIES COMPILE_FLAGS "${CMAKE_CXX_FLAGS_RELEASE}")
+
+endif()
+
+if (LIBCARLA_BUILD_DEBUG)
+
+  add_library(carla_fastdds_debug STATIC ${libcarla_fastdds_sources})
+ target_include_directories(carla_fastdds_debug SYSTEM PRIVATE
+      "${BOOST_INCLUDE_PATH}"
+      "${RPCLIB_INCLUDE_PATH}")
+  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)
+
+endif()

LibCarla/cmake/server/CMakeLists.txt

@@ -71,6 +71,9 @@ install(FILES ${libcarla_carla_streaming_low_level_headers} DESTINATION include/
 file(GLOB libcarla_carla_multigpu_headers "${libcarla_source_path}/carla/multigpu/*.h")
 install(FILES ${libcarla_carla_multigpu_headers} DESTINATION include/carla/multigpu)
 
+file(GLOB libcarla_carla_ros2_headers "${libcarla_source_path}/carla/ros2/*.h")
+install(FILES ${libcarla_carla_ros2_headers} DESTINATION include/carla/ros2)
+
 install(DIRECTORY "${BOOST_INCLUDE_PATH}/boost" DESTINATION include)
 
 if(WIN32)
@@ -115,6 +118,8 @@ file(GLOB libcarla_server_sources
     "${libcarla_source_path}/carla/streaming/low_level/*.h"
     "${libcarla_source_path}/carla/multigpu/*.h"
     "${libcarla_source_path}/carla/multigpu/*.cpp"
+    "${libcarla_source_path}/carla/ros2/*.h"
+    "${libcarla_source_path}/carla/ros2/*.cpp"
     "${libcarla_source_thirdparty_path}/odrSpiral/*.cpp"
     "${libcarla_source_thirdparty_path}/odrSpiral/*.h"
     "${libcarla_source_thirdparty_path}/moodycamel/*.cpp"

LibCarla/source/carla/Buffer.h

@@ -26,6 +26,7 @@
 namespace carla {
 
   class BufferPool;
+  class BufferView;
 
   /// A piece of raw data.
   ///
@@ -360,6 +361,7 @@ namespace carla {
     void ReuseThisBuffer();
 
     friend class BufferPool;
+    friend class BufferView;
 
     std::weak_ptr<BufferPool> _parent_pool;
 

LibCarla/source/carla/BufferView.h

@@ -0,0 +1,153 @@
+// Copyright (c) 2023 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 <https://opensource.org/licenses/MIT>.
+
+#pragma once
+
+#include "carla/Buffer.h"
+#include "carla/Debug.h"
+#include "carla/Exception.h"
+#include "carla/Logging.h"
+
+#include <boost/asio/buffer.hpp>
+
+#include <cstdint>
+#include <limits>
+#include <memory>
+#include <type_traits>
+
+#ifdef LIBCARLA_INCLUDED_FROM_UE4
+#include <compiler/enable-ue4-macros.h>
+#include "Containers/Array.h"
+#include <compiler/disable-ue4-macros.h>
+#endif // LIBCARLA_INCLUDED_FROM_UE4
+
+namespace carla {
+
+  class BufferPool;
+
+  /// Creating a constant view from an existing buffer
+  class BufferView : public std::enable_shared_from_this<BufferView> {
+
+    // =========================================================================
+    /// @name Member types
+    // =========================================================================
+    /// @{
+
+  public:
+
+    using value_type = unsigned char;
+    using size_type = uint32_t;
+    using const_iterator = const value_type *;
+
+    /// @}
+    // =========================================================================
+    /// @name Construction and destruction
+    // =========================================================================
+    /// @{
+
+  public:
+
+    BufferView() = delete;
+    BufferView(const BufferView &) = delete;
+
+    static std::shared_ptr<BufferView> CreateFrom(Buffer &&buffer) {
+      return std::shared_ptr<BufferView>(new BufferView(std::move(buffer)));
+    }
+
+  private:
+
+    BufferView(Buffer &&rhs) noexcept
+      : _buffer(std::move(rhs)) {}
+
+    /// @}
+    // =========================================================================
+    /// @name Data access
+    // =========================================================================
+    /// @{
+
+  public:
+
+    /// Access the byte at position @a i.
+    const value_type &operator[](size_t i) const {
+      return _buffer.data()[i];
+    }
+
+    /// Direct access to the allocated memory or nullptr if no memory is
+    /// allocated.
+    const value_type *data() const noexcept {
+      return _buffer.data();
+    }
+
+    /// Make a boost::asio::buffer from this buffer.
+    ///
+    /// @warning Boost.Asio buffers do not own the data, it's up to the caller
+    /// to not delete the memory that this buffer holds until the asio buffer is
+    /// no longer used.
+    boost::asio::const_buffer cbuffer() const noexcept {
+      return {_buffer.data(), _buffer.size()};
+    }
+
+    /// @copydoc cbuffer()
+    boost::asio::const_buffer buffer() const noexcept {
+      return cbuffer();
+    }
+
+    /// @}
+    // =========================================================================
+    /// @name Capacity
+    // =========================================================================
+    /// @{
+
+  public:
+
+    bool empty() const noexcept {
+      return _buffer.size() == 0u;
+    }
+
+    size_type size() const noexcept {
+      return _buffer.size();
+    }
+
+    static constexpr size_type max_size() noexcept {
+      return (std::numeric_limits<size_type>::max)();
+    }
+
+    size_type capacity() const noexcept {
+      return _buffer.capacity();
+    }
+
+    /// @}
+    // =========================================================================
+    /// @name Iterators
+    // =========================================================================
+    /// @{
+
+  public:
+
+    const_iterator cbegin() const noexcept {
+      return _buffer.data();
+    }
+
+    const_iterator begin() const noexcept {
+      return _buffer.cbegin();
+    }
+
+    const_iterator cend() const noexcept {
+      return _buffer.cbegin() + _buffer.size();
+    }
+
+    const_iterator end() const noexcept {
+      return _buffer.cend();
+    }
+
+  private:
+
+    const Buffer _buffer;
+  };
+
+  using SharedBufferView = std::shared_ptr<BufferView>;
+
+} // namespace carla

LibCarla/source/carla/client/ServerSideSensor.cpp

@@ -80,6 +80,18 @@ namespace client {
     listening_mask.reset(GBufferId + 1);
   }
 
+  void ServerSideSensor::EnableForROS() {
+    GetEpisode().Lock()->EnableForROS(*this);
+  }
+
+  void ServerSideSensor::DisableForROS() {
+    GetEpisode().Lock()->DisableForROS(*this);
+  }
+
+  bool ServerSideSensor::IsEnabledForROS(){
+    return GetEpisode().Lock()->IsEnabledForROS(*this);
+  }
+
   bool ServerSideSensor::Destroy() {
     log_debug("calling sensor Destroy() ", GetDisplayId());
     if (IsListening()) {

LibCarla/source/carla/client/ServerSideSensor.h

@@ -47,6 +47,15 @@ namespace client {
       return listening_mask.test(id + 1);
     }
 
+    /// Enable this sensor for ROS2 publishing
+    void EnableForROS();
+
+    /// Disable this sensor for ROS2 publishing
+    void DisableForROS();
+
+    /// Return if the sensor is publishing for ROS2
+    bool IsEnabledForROS();
+
     /// @copydoc Actor::Destroy()
     ///
     /// Additionally stop listening.
@@ -55,7 +64,6 @@ namespace client {
   private:
 
     std::bitset<16> listening_mask;
-
   };
 
 } // namespace client

LibCarla/source/carla/client/detail/Client.cpp

@@ -598,6 +598,21 @@ namespace detail {
     _pimpl->streaming_client.UnSubscribe(token);
   }
 
+  void Client::EnableForROS(const streaming::Token &token) {
+    carla::streaming::detail::token_type thisToken(token);
+    _pimpl->AsyncCall("enable_sensor_for_ros", thisToken.get_stream_id());
+  }
+
+  void Client::DisableForROS(const streaming::Token &token) {
+    carla::streaming::detail::token_type thisToken(token);
+    _pimpl->AsyncCall("disable_sensor_for_ros", thisToken.get_stream_id());
+  }
+
+  bool Client::IsEnabledForROS(const streaming::Token &token) {
+    carla::streaming::detail::token_type thisToken(token);
+    return _pimpl->CallAndWait<bool>("is_sensor_enabled_for_ros", thisToken.get_stream_id());
+  }
+
   void Client::SubscribeToGBuffer(
       rpc::ActorId ActorId,
       uint32_t GBufferId,

LibCarla/source/carla/client/detail/Client.h

@@ -384,6 +384,12 @@ namespace detail {
 
     void UnSubscribeFromStream(const streaming::Token &token);
 
+    void EnableForROS(const streaming::Token &token);
+
+    void DisableForROS(const streaming::Token &token);
+
+    bool IsEnabledForROS(const streaming::Token &token);
+
     void UnSubscribeFromGBuffer(
         rpc::ActorId ActorId,
         uint32_t GBufferId);

LibCarla/source/carla/client/detail/Simulator.cpp

@@ -61,7 +61,7 @@ namespace detail {
     if(result) {
       carla::traffic_manager::TrafficManager::Tick();
     }
-    
+
     return result;
   }
 
@@ -220,10 +220,10 @@ EpisodeProxy Simulator::GetCurrentEpisode() {
 
   uint64_t Simulator::Tick(time_duration timeout) {
     DEBUG_ASSERT(_episode != nullptr);
-    
+
     // tick pedestrian navigation
     NavigationTick();
-    
+
     // send tick command
     const auto frame = _client.SendTickCue();
 
@@ -401,12 +401,24 @@ EpisodeProxy Simulator::GetCurrentEpisode() {
           cb(std::move(data));
         });
   }
-  
+
   void Simulator::UnSubscribeFromSensor(Actor &sensor) {
     _client.UnSubscribeFromStream(sensor.GetActorDescription().GetStreamToken());
     // If in the future we need to unsubscribe from each gbuffer individually, it should be done here.
   }
 
+  void Simulator::EnableForROS(const Sensor &sensor) {
+    _client.EnableForROS(sensor.GetActorDescription().GetStreamToken());
+  }
+
+  void Simulator::DisableForROS(const Sensor &sensor) {
+    _client.DisableForROS(sensor.GetActorDescription().GetStreamToken());
+  }
+
+  bool Simulator::IsEnabledForROS(const Sensor &sensor) {
+    return _client.IsEnabledForROS(sensor.GetActorDescription().GetStreamToken());
+  }
+
   void Simulator::SubscribeToGBuffer(
       Actor &actor,
       uint32_t gbuffer_id,

LibCarla/source/carla/client/detail/Simulator.h

@@ -360,7 +360,7 @@ namespace detail {
         GarbageCollectionPolicy gc = GarbageCollectionPolicy::Inherit);
 
     bool DestroyActor(Actor &actor);
-    
+
     bool DestroyActor(ActorId actor_id)
     {
       return _client.DestroyActor(actor_id);
@@ -631,6 +631,12 @@ namespace detail {
 
     void UnSubscribeFromSensor(Actor &sensor);
 
+    void EnableForROS(const Sensor &sensor);
+
+    void DisableForROS(const Sensor &sensor);
+
+    bool IsEnabledForROS(const Sensor &sensor);
+
     void SubscribeToGBuffer(
         Actor & sensor,
         uint32_t gbuffer_id,

LibCarla/source/carla/multigpu/commands.h

@@ -15,6 +15,9 @@ enum MultiGPUCommand : uint32_t {
   SEND_FRAME = 0,
   LOAD_MAP,
   GET_TOKEN,
+  ENABLE_ROS,
+  DISABLE_ROS,
+  IS_ENABLED_ROS,
   YOU_ALIVE
 };
 

LibCarla/source/carla/multigpu/primary.h

@@ -51,11 +51,11 @@ namespace multigpu {
         Listener::callback_function_type_response on_response);
 
     template <typename... Buffers>
-    static auto MakeMessage(Buffers &&... buffers) {
+    static auto MakeMessage(Buffers... buffers) {
       static_assert(
-          are_same<Buffer, Buffers...>::value,
-          "This function only accepts arguments of type Buffer.");
-      return std::make_shared<const carla::streaming::detail::tcp::Message>(std::move(buffers)...);
+          are_same<SharedBufferView, Buffers...>::value,
+          "This function only accepts arguments of type BufferView.");
+      return std::make_shared<const carla::streaming::detail::tcp::Message>(buffers...);
     }
 
     /// Writes some data to the socket.
@@ -69,8 +69,8 @@ namespace multigpu {
 
     /// Writes some data to the socket.
     template <typename... Buffers>
-    void Write(Buffers &&... buffers) {
-      Write(MakeMessage(std::move(buffers)...));
+    void Write(Buffers... buffers) {
+      Write(MakeMessage(buffers...));
     }
 
     /// Post a job to close the session.

LibCarla/source/carla/multigpu/primaryCommands.cpp

@@ -24,6 +24,10 @@ PrimaryCommands::PrimaryCommands(std::shared_ptr<Router> router) :
   _router(router) {
 }
 
+void PrimaryCommands::set_router(std::shared_ptr<Router> router) {
+  _router = router;
+}
+
 // broadcast to all secondary servers the frame data
 void PrimaryCommands::SendFrameData(carla::Buffer buffer) {
   _router->Write(MultiGPUCommand::SEND_FRAME, std::move(buffer));
@@ -37,10 +41,10 @@ void PrimaryCommands::SendLoadMap(std::string map) {
 }
 
 // send to who the router wants the request for a token
-token_type PrimaryCommands::SendGetToken(carla::streaming::detail::stream_id_type sensor_id) {
+token_type PrimaryCommands::SendGetToken(stream_id sensor_id) {
   log_info("asking for a token");
-  carla::Buffer buf((carla::Buffer::value_type *) &sensor_id, 
-                    (size_t) sizeof(carla::streaming::detail::stream_id_type));
+  carla::Buffer buf((carla::Buffer::value_type *) &sensor_id,
+                    (size_t) sizeof(stream_id));
   auto fut = _router->WriteToNext(MultiGPUCommand::GET_TOKEN, std::move(buf));
 
   auto response = fut.get();
@@ -59,8 +63,97 @@ void PrimaryCommands::SendIsAlive() {
   log_info("response from alive command: ", response.buffer.data());
 }
 
-void PrimaryCommands::set_router(std::shared_ptr<Router> router) {
-  _router = router;
+void PrimaryCommands::SendEnableForROS(stream_id sensor_id) {
+  // search if the sensor has been activated in any secondary server
+  auto it = _servers.find(sensor_id);
+  if (it != _servers.end()) {
+    carla::Buffer buf((carla::Buffer::value_type *) &sensor_id,
+                      (size_t) sizeof(stream_id));
+    auto fut = _router->WriteToOne(it->second, MultiGPUCommand::ENABLE_ROS, std::move(buf));
+
+    auto response = fut.get();
+    bool res = (*reinterpret_cast<bool *>(response.buffer.data()));
+  } else {
+    log_error("enable_for_ros for sensor", sensor_id, " not found on any server");
+  }
+}
+
+void PrimaryCommands::SendDisableForROS(stream_id sensor_id) {
+  // search if the sensor has been activated in any secondary server
+  auto it = _servers.find(sensor_id);
+  if (it != _servers.end()) {
+    carla::Buffer buf((carla::Buffer::value_type *) &sensor_id,
+                      (size_t) sizeof(stream_id));
+    auto fut = _router->WriteToOne(it->second, MultiGPUCommand::DISABLE_ROS, std::move(buf));
+
+    auto response = fut.get();
+    bool res = (*reinterpret_cast<bool *>(response.buffer.data()));
+  } else {
+    log_error("disable_for_ros for sensor", sensor_id, " not found on any server");
+  }
+}
+
+bool PrimaryCommands::SendIsEnabledForROS(stream_id sensor_id) {
+  // search if the sensor has been activated in any secondary server
+  auto it = _servers.find(sensor_id);
+  if (it != _servers.end()) {
+    carla::Buffer buf((carla::Buffer::value_type *) &sensor_id,
+                      (size_t) sizeof(stream_id));
+    auto fut = _router->WriteToOne(it->second, MultiGPUCommand::IS_ENABLED_ROS, std::move(buf));
+
+    auto response = fut.get();
+    bool res = (*reinterpret_cast<bool *>(response.buffer.data()));
+    return res;
+  } else {
+    log_error("is_enabled_for_ros for sensor", sensor_id, " not found on any server");
+    return false;
+  }
+}
+
+token_type PrimaryCommands::GetToken(stream_id sensor_id) {
+  // search if the sensor has been activated in any secondary server
+  auto it = _tokens.find(sensor_id);
+  if (it != _tokens.end()) {
+    // return already activated sensor token
+    log_debug("Using token from already activated sensor: ", it->second.get_stream_id(), ", ", it->second.get_port());
+    return it->second;
+  }
+  else {
+    // enable the sensor on one secondary server
+    auto server = _router->GetNextServer();
+    auto token = SendGetToken(sensor_id);
+    // add to the maps
+    _tokens[sensor_id] = token;
+    _servers[sensor_id] = server;
+    log_debug("Using token from new activated sensor: ", token.get_stream_id(), ", ", token.get_port());
+    return token;
+  }
+}
+
+void PrimaryCommands::EnableForROS(stream_id sensor_id) {
+  auto it = _servers.find(sensor_id);
+  if (it != _servers.end()) {
+    SendEnableForROS(sensor_id);
+  } else {
+    // we need to activate the sensor in any server yet, and repeat
+    GetToken(sensor_id);
+    EnableForROS(sensor_id);
+  }
+}
+
+void PrimaryCommands::DisableForROS(stream_id sensor_id) {
+  auto it = _servers.find(sensor_id);
+  if (it != _servers.end()) {
+    SendDisableForROS(sensor_id);
+  }
+}
+
+bool PrimaryCommands::IsEnabledForROS(stream_id sensor_id) {
+  auto it = _servers.find(sensor_id);
+  if (it != _servers.end()) {
+    return SendIsEnabledForROS(sensor_id);
+  }
+  return false;
 }
 
 } // namespace multigpu

LibCarla/source/carla/multigpu/primaryCommands.h

@@ -19,33 +19,50 @@ namespace multigpu {
 // using session = std::shared_ptr<Primary>;
 // using callback_response = std::function<void(std::shared_ptr<Primary>, carla::Buffer)>;
 using token_type = carla::streaming::detail::token_type;
+using stream_id = carla::streaming::detail::stream_id_type;
 
 class Router;
 
 class PrimaryCommands {
   public:
-    
+
 
     PrimaryCommands();
     PrimaryCommands(std::shared_ptr<Router> router);
 
+    void set_router(std::shared_ptr<Router> router);
+
     // broadcast to all secondary servers the frame data
     void SendFrameData(carla::Buffer buffer);
-    
+
     // broadcast to all secondary servers the map to load
     void SendLoadMap(std::string map);
 
-    // send to one secondary to get the token of a sensor
-    token_type SendGetToken(carla::streaming::detail::stream_id_type sensor_id);
-    
     // send to know if a connection is alive
     void SendIsAlive();
 
-    void set_router(std::shared_ptr<Router> router);
+    token_type GetToken(stream_id sensor_id);
+
+    void EnableForROS(stream_id sensor_id);
+
+    void DisableForROS(stream_id sensor_id);
+
+    bool IsEnabledForROS(stream_id sensor_id);
 
   private:
 
+    // send to one secondary to get the token of a sensor
+    token_type SendGetToken(carla::streaming::detail::stream_id_type sensor_id);
+
+    // manage ROS enable/disable of sensor
+    void SendEnableForROS(stream_id sensor_id);
+    void SendDisableForROS(stream_id sensor_id);
+    bool SendIsEnabledForROS(stream_id sensor_id);
+
+
     std::shared_ptr<Router> _router;
+    std::unordered_map<stream_id, token_type> _tokens;
+    std::unordered_map<stream_id, std::weak_ptr<Primary>> _servers;
 };
 
 } // namespace multigpu

LibCarla/source/carla/multigpu/router.cpp

@@ -27,7 +27,7 @@ void Router::Stop() {
 }
 
 Router::Router(uint16_t port) :
-  _next(0) { 
+  _next(0) {
 
   _endpoint = boost::asio::ip::tcp::endpoint(boost::asio::ip::address::from_string("0.0.0.0"), port);
   _listener = std::make_shared<carla::multigpu::Listener>(_pool.io_context(), _endpoint);
@@ -37,20 +37,20 @@ void Router::SetCallbacks() {
   // prepare server
   std::weak_ptr<Router> weak = shared_from_this();
 
-  carla::multigpu::Listener::callback_function_type on_open = [=](std::shared_ptr<carla::multigpu::Primary> session) { 
+  carla::multigpu::Listener::callback_function_type on_open = [=](std::shared_ptr<carla::multigpu::Primary> session) {
     auto self = weak.lock();
     if (!self) return;
     self->ConnectSession(session);
   };
 
-  carla::multigpu::Listener::callback_function_type on_close = [=](std::shared_ptr<carla::multigpu::Primary> session) { 
+  carla::multigpu::Listener::callback_function_type on_close = [=](std::shared_ptr<carla::multigpu::Primary> session) {
     auto self = weak.lock();
     if (!self) return;
-    self->DisconnectSession(session); 
+    self->DisconnectSession(session);
   };
 
-  carla::multigpu::Listener::callback_function_type_response on_response = 
-    [=](std::shared_ptr<carla::multigpu::Primary> session, carla::Buffer buffer) { 
+  carla::multigpu::Listener::callback_function_type_response on_response =
+    [=](std::shared_ptr<carla::multigpu::Primary> session, carla::Buffer buffer) {
       auto self = weak.lock();
       if (!self) return;
       std::lock_guard<std::mutex> lock(self->_mutex);
@@ -89,7 +89,7 @@ void Router::ConnectSession(std::shared_ptr<Primary> session) {
   _sessions.emplace_back(std::move(session));
   log_info("Connected secondary servers:", _sessions.size());
   // run external callback for new connections
-  if (_callback) 
+  if (_callback)
     _callback();
 }
 
@@ -115,9 +115,11 @@ void Router::Write(MultiGPUCommand id, Buffer &&buffer) {
   header.id = id;
   header.size = buffer.size();
   Buffer buf_header((uint8_t *) &header, sizeof(header));
-  
-  auto message = Primary::MakeMessage(std::move(buf_header), std::move(buffer));
-  
+
+  auto view_header = carla::BufferView::CreateFrom(std::move(buf_header));
+  auto view_data = carla::BufferView::CreateFrom(std::move(buffer));
+  auto message = Primary::MakeMessage(view_header, view_data);
+
   // write to multiple servers
   std::lock_guard<std::mutex> lock(_mutex);
   for (auto &s : _sessions) {
@@ -134,7 +136,9 @@ std::future<SessionInfo> Router::WriteToNext(MultiGPUCommand id, Buffer &&buffer
   header.size = buffer.size();
   Buffer buf_header((uint8_t *) &header, sizeof(header));
 
-  auto message = Primary::MakeMessage(std::move(buf_header), std::move(buffer));
+  auto view_header = carla::BufferView::CreateFrom(std::move(buf_header));
+  auto view_data = carla::BufferView::CreateFrom(std::move(buffer));
+  auto message = Primary::MakeMessage(view_header, view_data);
 
   // create the promise for the posible answer
   auto response = std::make_shared<std::promise<SessionInfo>>();
@@ -157,5 +161,41 @@ std::future<SessionInfo> Router::WriteToNext(MultiGPUCommand id, Buffer &&buffer
   return response->get_future();
 }
 
+std::future<SessionInfo> Router::WriteToOne(std::weak_ptr<Primary> server, MultiGPUCommand id, Buffer &&buffer) {
+  // define the command header
+  CommandHeader header;
+  header.id = id;
+  header.size = buffer.size();
+  Buffer buf_header((uint8_t *) &header, sizeof(header));
+
+  auto view_header = carla::BufferView::CreateFrom(std::move(buf_header));
+  auto view_data = carla::BufferView::CreateFrom(std::move(buffer));
+  auto message = Primary::MakeMessage(view_header, view_data);
+
+  // create the promise for the posible answer
+  auto response = std::make_shared<std::promise<SessionInfo>>();
+
+  // write to the specific server only
+  std::lock_guard<std::mutex> lock(_mutex);
+  auto s = server.lock();
+  if (s) {
+    _promises[s.get()] = response;
+    s->Write(message);
+  }
+  return response->get_future();
+}
+
+std::weak_ptr<Primary> Router::GetNextServer() {
+  std::lock_guard<std::mutex> lock(_mutex);
+  if (_next >= _sessions.size()) {
+    _next = 0;
+  }
+  if (_next < _sessions.size()) {
+    return std::weak_ptr<Primary>(_sessions[_next]);
+  } else {
+    return std::weak_ptr<Primary>();
+  }
+}
+
 } // namespace multigpu
 } // namespace carla

LibCarla/source/carla/multigpu/router.h

@@ -38,9 +38,10 @@ namespace multigpu {
     Router(void);
     explicit Router(uint16_t port);
     ~Router();
-    
+
     void Write(MultiGPUCommand id, Buffer &&buffer);
     std::future<SessionInfo> WriteToNext(MultiGPUCommand id, Buffer &&buffer);
+    std::future<SessionInfo> WriteToOne(std::weak_ptr<Primary> server, MultiGPUCommand id, Buffer &&buffer);
     void Stop();
 
     void SetCallbacks();
@@ -58,11 +59,13 @@ namespace multigpu {
       return _commander;
     }
 
+    std::weak_ptr<Primary> GetNextServer();
+
   private:
     void ConnectSession(std::shared_ptr<Primary> session);
     void DisconnectSession(std::shared_ptr<Primary> session);
     void ClearSessions();
-    
+
     // mutex and thread pool must be at the beginning to be destroyed last
     std::mutex                              _mutex;
     ThreadPool                              _pool;
@@ -70,7 +73,7 @@ namespace multigpu {
     std::vector<std::shared_ptr<Primary>>   _sessions;
     std::shared_ptr<Listener>               _listener;
     uint32_t                                _next;
-    std::unordered_map<Primary *, std::shared_ptr<std::promise<SessionInfo>>>   _promises;
+    std::unordered_map<Primary *, std::shared_ptr<std::promise<SessionInfo>>> _promises;
     PrimaryCommands                         _commander;
     std::function<void(void)>               _callback;
   };

LibCarla/source/carla/multigpu/secondary.cpp

@@ -33,10 +33,10 @@ namespace multigpu {
       _strand(_pool.io_context()),
       _connection_timer(_pool.io_context()),
       _buffer_pool(std::make_shared<BufferPool>()) {
-        
+
       _commander.set_callback(callback);
     }
-  
+
 
   Secondary::Secondary(
     std::string ip,
@@ -92,7 +92,7 @@ namespace multigpu {
         // This forces not using Nagle's algorithm.
         // Improves the sync mode velocity on Linux by a factor of ~3.
         self->_socket.set_option(boost::asio::ip::tcp::no_delay(true));
-        
+
         log_info("secondary server: connected to ", self->_endpoint);
 
         self->ReadData();
@@ -157,9 +157,10 @@ namespace multigpu {
           boost::asio::bind_executor(self->_strand, handle_sent));
     });
   }
-  
+
   void Secondary::Write(Buffer buffer) {
-    auto message = Secondary::MakeMessage(std::move(buffer));
+    auto view_data = carla::BufferView::CreateFrom(std::move(buffer));
+    auto message = Secondary::MakeMessage(view_data);
 
     DEBUG_ASSERT(message != nullptr);
     DEBUG_ASSERT(!message->empty());
@@ -186,7 +187,7 @@ namespace multigpu {
           boost::asio::bind_executor(self->_strand, handle_sent));
     });
   }
-  
+
   void Secondary::Write(std::string text) {
     std::weak_ptr<Secondary> weak = shared_from_this();
     boost::asio::post(_strand, [=]() {
@@ -211,7 +212,7 @@ namespace multigpu {
           self->_socket,
           boost::asio::buffer(&this_size, sizeof(this_size)),
           boost::asio::bind_executor(self->_strand, handle_sent));
-      
+
       // send characters
       boost::asio::async_write(
           self->_socket,

LibCarla/source/carla/multigpu/secondary.h

@@ -59,11 +59,11 @@ namespace multigpu {
     }
 
     template <typename... Buffers>
-    static auto MakeMessage(Buffers &&... buffers) {
+    static auto MakeMessage(Buffers... buffers) {
       static_assert(
-          are_same<Buffer, Buffers...>::value,
-          "This function only accepts arguments of type Buffer.");
-      return std::make_shared<const carla::streaming::detail::tcp::Message>(std::move(buffers)...);
+          are_same<SharedBufferView, Buffers...>::value,
+          "This function only accepts arguments of type BufferView.");
+      return std::make_shared<const carla::streaming::detail::tcp::Message>(buffers...);
     }
 
   private:

LibCarla/source/carla/ros2/ROS2.cpp

@@ -0,0 +1,847 @@
+// Copyright (c) 2023 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 <https://opensource.org/licenses/MIT>.
+
+#include "carla/Logging.h"
+#include "carla/ros2/ROS2.h"
+#include "carla/geom/GeoLocation.h"
+#include "carla/geom/Vector3D.h"
+#include "carla/sensor/data/DVSEvent.h"
+#include "carla/sensor/data/LidarData.h"
+#include "carla/sensor/data/SemanticLidarData.h"
+#include "carla/sensor/data/RadarData.h"
+#include "carla/sensor/data/Image.h"
+#include "carla/sensor/s11n/ImageSerializer.h"
+#include "carla/sensor/s11n/SensorHeaderSerializer.h"
+
+#include "publishers/CarlaPublisher.h"
+#include "publishers/CarlaClockPublisher.h"
+#include "publishers/CarlaRGBCameraPublisher.h"
+#include "publishers/CarlaDepthCameraPublisher.h"
+#include "publishers/CarlaNormalsCameraPublisher.h"
+#include "publishers/CarlaOpticalFlowCameraPublisher.h"
+#include "publishers/CarlaSSCameraPublisher.h"
+#include "publishers/CarlaISCameraPublisher.h"
+#include "publishers/CarlaDVSCameraPublisher.h"
+#include "publishers/CarlaLidarPublisher.h"
+#include "publishers/CarlaSemanticLidarPublisher.h"
+#include "publishers/CarlaRadarPublisher.h"
+#include "publishers/CarlaIMUPublisher.h"
+#include "publishers/CarlaGNSSPublisher.h"
+#include "publishers/CarlaMapSensorPublisher.h"
+#include "publishers/CarlaSpeedometerSensor.h"
+#include "publishers/CarlaTransformPublisher.h"
+#include "publishers/CarlaCollisionPublisher.h"
+#include "publishers/CarlaLineInvasionPublisher.h"
+
+#include "subscribers/CarlaSubscriber.h"
+#include "subscribers/CarlaEgoVehicleControlSubscriber.h"
+
+#include <vector>
+
+namespace carla {
+namespace ros2 {
+
+// static fields
+std::shared_ptr<ROS2> ROS2::_instance;
+
+// list of sensors (should be equal to the list of SensorsRegistry
+enum ESensors {
+  CollisionSensor,
+  DepthCamera,
+  NormalsCamera,
+  DVSCamera,
+  GnssSensor,
+  InertialMeasurementUnit,
+  LaneInvasionSensor,
+  ObstacleDetectionSensor,
+  OpticalFlowCamera,
+  Radar,
+  RayCastSemanticLidar,
+  RayCastLidar,
+  RssSensor,
+  SceneCaptureCamera,
+  SemanticSegmentationCamera,
+  InstanceSegmentationCamera,
+  WorldObserver,
+  CameraGBufferUint8,
+  CameraGBufferFloat
+};
+
+void ROS2::Enable(bool enable) {
+  _enabled = enable;
+  log_info("ROS2 enabled: ", _enabled);
+  _clock_publisher = std::make_shared<CarlaClockPublisher>("clock", "");
+  _clock_publisher->Init();
+}
+
+void ROS2::SetFrame(uint64_t frame) {
+  _frame = frame;
+   //log_info("ROS2 new frame: ", _frame);
+   if (_controller) {
+    void* actor = _controller->GetVehicle();
+    if (_controller->IsAlive()) {
+      if (_controller->HasNewMessage()) {
+        auto it = _actor_callbacks.find(actor);
+        if (it != _actor_callbacks.end()) {
+          VehicleControl control = _controller->GetMessage();
+          it->second(actor, control);
+        }
+      }
+    } else {
+      RemoveActorCallback(actor);
+    }
+   }
+}
+
+void ROS2::SetTimestamp(double timestamp) {
+  double integral;
+  const double fractional = modf(timestamp, &integral);
+  const double multiplier = 1000000000.0;
+  _seconds = static_cast<int32_t>(integral);
+  _nanoseconds = static_cast<uint32_t>(fractional * multiplier);
+  _clock_publisher->SetData(_seconds, _nanoseconds);
+  _clock_publisher->Publish();
+   //log_info("ROS2 new timestamp: ", _timestamp);
+}
+
+void ROS2::AddActorRosName(void *actor, std::string ros_name) {
+  _actor_ros_name.insert({actor, ros_name});
+}
+
+void ROS2::AddActorParentRosName(void *actor, void* parent) {
+  auto it = _actor_parent_ros_name.find(actor);
+  if (it != _actor_parent_ros_name.end()) {
+    it->second.push_back(parent);
+  } else {
+    _actor_parent_ros_name.insert({actor, {parent}});
+  }
+}
+
+void ROS2::RemoveActorRosName(void *actor) {
+  _actor_ros_name.erase(actor);
+  _actor_parent_ros_name.erase(actor);
+
+  _publishers.erase(actor);
+  _transforms.erase(actor);
+}
+
+void ROS2::UpdateActorRosName(void *actor, std::string ros_name) {
+  auto it = _actor_ros_name.find(actor);
+  if (it != _actor_ros_name.end()) {
+    it->second = ros_name;
+  }
+}
+
+std::string ROS2::GetActorRosName(void *actor) {
+  auto it = _actor_ros_name.find(actor);
+  if (it != _actor_ros_name.end()) {
+    return it->second;
+  } else {
+    return std::string("");
+  }
+}
+
+std::string ROS2::GetActorParentRosName(void *actor) {
+  auto it = _actor_parent_ros_name.find(actor);
+  if (it != _actor_parent_ros_name.end())
+  {
+    const std::string current_actor_name = GetActorRosName(actor);
+    std::string parent_name;
+    for (auto parent_it = it->second.cbegin(); parent_it != it->second.cend(); ++parent_it)
+    {
+      const std::string name = GetActorRosName(*parent_it);
+      if (name == current_actor_name)
+      {
+        continue;
+      }
+      if (name.empty())
+      {
+        continue;
+      }
+      parent_name = name + '/' + parent_name;
+    }
+    if (parent_name.back() == '/')
+      parent_name.pop_back();
+    return parent_name;
+  }
+  else
+    return std::string("");
+}
+
+void ROS2::AddActorCallback(void* actor, std::string ros_name, ActorCallback callback) {
+  _actor_callbacks.insert({actor, std::move(callback)});
+
+  _controller.reset();
+  _controller = std::make_shared<CarlaEgoVehicleControlSubscriber>(actor, ros_name.c_str());
+  _controller->Init();
+}
+
+void ROS2::RemoveActorCallback(void* actor) {
+  _controller.reset();
+  _actor_callbacks.erase(actor);
+}
+
+std::pair<std::shared_ptr<CarlaPublisher>, std::shared_ptr<CarlaTransformPublisher>> ROS2::GetOrCreateSensor(int type, carla::streaming::detail::stream_id_type id, void* actor) {
+  auto it_publishers = _publishers.find(actor);
+  auto it_transforms = _transforms.find(actor);
+  std::shared_ptr<CarlaPublisher> publisher {};
+  std::shared_ptr<CarlaTransformPublisher> transform {};
+  if (it_publishers != _publishers.end()) {
+    publisher = it_publishers->second;
+    if (it_transforms != _transforms.end()) {
+      transform = it_transforms->second;
+    }
+  } else {
+    //Sensor not found, creating one of the given type
+    const std::string string_id = std::to_string(id);
+    std::string ros_name = GetActorRosName(actor);
+    std::string parent_ros_name = GetActorParentRosName(actor);
+    switch(type) {
+      case ESensors::CollisionSensor: {
+        if (ros_name == "collision__") {
+          ros_name.pop_back();
+          ros_name.pop_back();
+          ros_name += string_id;
+          UpdateActorRosName(actor, ros_name);
+        }
+        std::shared_ptr<CarlaCollisionPublisher> new_publisher = std::make_shared<CarlaCollisionPublisher>(ros_name.c_str(), parent_ros_name.c_str());
+        if (new_publisher->Init()) {
+          _publishers.insert({actor, new_publisher});
+          publisher = new_publisher;
+        }
+        std::shared_ptr<CarlaTransformPublisher> new_transform = std::make_shared<CarlaTransformPublisher>(ros_name.c_str(), parent_ros_name.c_str());
+        if (new_transform->Init()) {
+          _transforms.insert({actor, new_transform});
+          transform = new_transform;
+        }
+      } break;
+      case ESensors::DepthCamera: {
+        if (ros_name == "depth__") {
+          ros_name.pop_back();
+          ros_name.pop_back();
+          ros_name += string_id;
+          UpdateActorRosName(actor, ros_name);
+        }
+        std::shared_ptr<CarlaDepthCameraPublisher> new_publisher = std::make_shared<CarlaDepthCameraPublisher>(ros_name.c_str(), parent_ros_name.c_str());
+        if (new_publisher->Init()) {
+          _publishers.insert({actor, new_publisher});
+          publisher = new_publisher;
+        }
+        std::shared_ptr<CarlaTransformPublisher> new_transform = std::make_shared<CarlaTransformPublisher>(ros_name.c_str(), parent_ros_name.c_str());
+        if (new_transform->Init()) {
+          _transforms.insert({actor, new_transform});
+          transform = new_transform;
+        }
+      } break;
+      case ESensors::NormalsCamera: {
+        if (ros_name == "normals__") {
+          ros_name.pop_back();
+          ros_name.pop_back();
+          ros_name += string_id;
+          UpdateActorRosName(actor, ros_name);
+        }
+        std::shared_ptr<CarlaNormalsCameraPublisher> new_publisher = std::make_shared<CarlaNormalsCameraPublisher>(ros_name.c_str(), parent_ros_name.c_str());
+        if (new_publisher->Init()) {
+          _publishers.insert({actor, new_publisher});
+          publisher = new_publisher;
+        }
+        std::shared_ptr<CarlaTransformPublisher> new_transform = std::make_shared<CarlaTransformPublisher>(ros_name.c_str(), parent_ros_name.c_str());
+        if (new_transform->Init()) {
+          _transforms.insert({actor, new_transform});
+          transform = new_transform;
+        }
+      } break;
+      case ESensors::DVSCamera: {
+        if (ros_name == "dvs__") {
+          ros_name.pop_back();
+          ros_name.pop_back();
+          ros_name += string_id;
+          UpdateActorRosName(actor, ros_name);
+        }
+        std::shared_ptr<CarlaDVSCameraPublisher> new_publisher = std::make_shared<CarlaDVSCameraPublisher>(ros_name.c_str(), parent_ros_name.c_str());
+        if (new_publisher->Init()) {
+          _publishers.insert({actor, new_publisher});
+          publisher = new_publisher;
+        }
+        std::shared_ptr<CarlaTransformPublisher> new_transform = std::make_shared<CarlaTransformPublisher>(ros_name.c_str(), parent_ros_name.c_str());
+        if (new_transform->Init()) {
+          _transforms.insert({actor, new_transform});
+          transform = new_transform;
+        }
+      } break;
+      case ESensors::GnssSensor: {
+        if (ros_name == "gnss__") {
+          ros_name.pop_back();
+          ros_name.pop_back();
+          ros_name += string_id;
+          UpdateActorRosName(actor, ros_name);
+        }
+        std::shared_ptr<CarlaGNSSPublisher> new_publisher = std::make_shared<CarlaGNSSPublisher>(ros_name.c_str(), parent_ros_name.c_str());
+        if (new_publisher->Init()) {
+          _publishers.insert({actor, new_publisher});
+          publisher = new_publisher;
+        }
+        std::shared_ptr<CarlaTransformPublisher> new_transform = std::make_shared<CarlaTransformPublisher>(ros_name.c_str(), parent_ros_name.c_str());
+        if (new_transform->Init()) {
+          _transforms.insert({actor, new_transform});
+          transform = new_transform;
+        }
+      } break;
+      case ESensors::InertialMeasurementUnit: {
+        if (ros_name == "imu__") {
+          ros_name.pop_back();
+          ros_name.pop_back();
+          ros_name += string_id;
+          UpdateActorRosName(actor, ros_name);
+        }
+        std::shared_ptr<CarlaIMUPublisher> new_publisher = std::make_shared<CarlaIMUPublisher>(ros_name.c_str(), parent_ros_name.c_str());
+        if (new_publisher->Init()) {
+          _publishers.insert({actor, new_publisher});
+          publisher = new_publisher;
+        }
+        std::shared_ptr<CarlaTransformPublisher> new_transform = std::make_shared<CarlaTransformPublisher>(ros_name.c_str(), parent_ros_name.c_str());
+        if (new_transform->Init()) {
+          _transforms.insert({actor, new_transform});
+          transform = new_transform;
+        }
+      } break;
+      case ESensors::LaneInvasionSensor: {
+        if (ros_name == "lane_invasion__") {
+          ros_name.pop_back();
+          ros_name.pop_back();
+          ros_name += string_id;
+          UpdateActorRosName(actor, ros_name);
+        }
+        std::shared_ptr<CarlaLineInvasionPublisher> new_publisher = std::make_shared<CarlaLineInvasionPublisher>(ros_name.c_str(), parent_ros_name.c_str());
+        if (new_publisher->Init()) {
+          _publishers.insert({actor, new_publisher});
+          publisher = new_publisher;
+        }
+        std::shared_ptr<CarlaTransformPublisher> new_transform = std::make_shared<CarlaTransformPublisher>(ros_name.c_str(), parent_ros_name.c_str());
+        if (new_transform->Init()) {
+          _transforms.insert({actor, new_transform});
+          transform = new_transform;
+        }
+      } break;
+      case ESensors::ObstacleDetectionSensor: {
+        std::cerr << "Obstacle detection sensor does not have an available publisher" << std::endl;
+      } break;
+      case ESensors::OpticalFlowCamera: {
+        if (ros_name == "optical_flow__") {
+          ros_name.pop_back();
+          ros_name.pop_back();
+          ros_name += string_id;
+          UpdateActorRosName(actor, ros_name);
+        }
+        std::shared_ptr<CarlaOpticalFlowCameraPublisher> new_publisher = std::make_shared<CarlaOpticalFlowCameraPublisher>(ros_name.c_str(), parent_ros_name.c_str());
+        if (new_publisher->Init()) {
+          _publishers.insert({actor, new_publisher});
+          publisher = new_publisher;
+        }
+        std::shared_ptr<CarlaTransformPublisher> new_transform = std::make_shared<CarlaTransformPublisher>(ros_name.c_str(), parent_ros_name.c_str());
+        if (new_transform->Init()) {
+          _transforms.insert({actor, new_transform});
+          transform = new_transform;
+        }
+      } break;
+      case ESensors::Radar: {
+        if (ros_name == "radar__") {
+          ros_name.pop_back();
+          ros_name.pop_back();
+          ros_name += string_id;
+          UpdateActorRosName(actor, ros_name);
+        }
+        std::shared_ptr<CarlaRadarPublisher> new_publisher = std::make_shared<CarlaRadarPublisher>(ros_name.c_str(), parent_ros_name.c_str());
+        if (new_publisher->Init()) {
+          _publishers.insert({actor, new_publisher});
+          publisher = new_publisher;
+        }
+        std::shared_ptr<CarlaTransformPublisher> new_transform = std::make_shared<CarlaTransformPublisher>(ros_name.c_str(), parent_ros_name.c_str());
+        if (new_transform->Init()) {
+          _transforms.insert({actor, new_transform});
+          transform = new_transform;
+        }
+      } break;
+      case ESensors::RayCastSemanticLidar: {
+        if (ros_name == "ray_cast_semantic__") {
+          ros_name.pop_back();
+          ros_name.pop_back();
+          ros_name += string_id;
+          UpdateActorRosName(actor, ros_name);
+        }
+        std::shared_ptr<CarlaSemanticLidarPublisher> new_publisher = std::make_shared<CarlaSemanticLidarPublisher>(ros_name.c_str(), parent_ros_name.c_str());
+        if (new_publisher->Init()) {
+          _publishers.insert({actor, new_publisher});
+          publisher = new_publisher;
+        }
+        std::shared_ptr<CarlaTransformPublisher> new_transform = std::make_shared<CarlaTransformPublisher>(ros_name.c_str(), parent_ros_name.c_str());
+        if (new_transform->Init()) {
+          _transforms.insert({actor, new_transform});
+          transform = new_transform;
+        }
+      } break;
+      case ESensors::RayCastLidar: {
+        if (ros_name == "ray_cast__") {
+          ros_name.pop_back();
+          ros_name.pop_back();
+          ros_name += string_id;
+          UpdateActorRosName(actor, ros_name);
+        }
+        std::shared_ptr<CarlaLidarPublisher> new_publisher = std::make_shared<CarlaLidarPublisher>(ros_name.c_str(), parent_ros_name.c_str());
+        if (new_publisher->Init()) {
+          _publishers.insert({actor, new_publisher});
+          publisher = new_publisher;
+        }
+        std::shared_ptr<CarlaTransformPublisher> new_transform = std::make_shared<CarlaTransformPublisher>(ros_name.c_str(), parent_ros_name.c_str());
+        if (new_transform->Init()) {
+          _transforms.insert({actor, new_transform});
+          transform = new_transform;
+        }
+      } break;
+      case ESensors::RssSensor: {
+        std::cerr << "RSS sensor does not have an available publisher" << std::endl;
+      } break;
+      case ESensors::SceneCaptureCamera: {
+        if (ros_name == "rgb__") {
+          ros_name.pop_back();
+          ros_name.pop_back();
+          ros_name += string_id;
+          UpdateActorRosName(actor, ros_name);
+        }
+        std::shared_ptr<CarlaRGBCameraPublisher> new_publisher = std::make_shared<CarlaRGBCameraPublisher>(ros_name.c_str(), parent_ros_name.c_str());
+        if (new_publisher->Init()) {
+          _publishers.insert({actor, new_publisher});
+          publisher = new_publisher;
+        }
+        std::shared_ptr<CarlaTransformPublisher> new_transform = std::make_shared<CarlaTransformPublisher>(ros_name.c_str(), parent_ros_name.c_str());
+        if (new_transform->Init()) {
+          _transforms.insert({actor, new_transform});
+          transform = new_transform;
+        }
+      } break;
+      case ESensors::SemanticSegmentationCamera: {
+        if (ros_name == "semantic_segmentation__") {
+          ros_name.pop_back();
+          ros_name.pop_back();
+          ros_name += string_id;
+          UpdateActorRosName(actor, ros_name);
+        }
+        std::shared_ptr<CarlaSSCameraPublisher> new_publisher = std::make_shared<CarlaSSCameraPublisher>(ros_name.c_str(), parent_ros_name.c_str());
+        if (new_publisher->Init()) {
+          _publishers.insert({actor, new_publisher});
+          publisher = new_publisher;
+        }
+        std::shared_ptr<CarlaTransformPublisher> new_transform = std::make_shared<CarlaTransformPublisher>(ros_name.c_str(), parent_ros_name.c_str());
+        if (new_transform->Init()) {
+          _transforms.insert({actor, new_transform});
+          transform = new_transform;
+        }
+      } break;
+      case ESensors::InstanceSegmentationCamera: {
+        if (ros_name == "instance_segmentation__") {
+          ros_name.pop_back();
+          ros_name.pop_back();
+          ros_name += string_id;
+          UpdateActorRosName(actor, ros_name);
+        }
+        std::shared_ptr<CarlaISCameraPublisher> new_publisher = std::make_shared<CarlaISCameraPublisher>(ros_name.c_str(), parent_ros_name.c_str());
+        if (new_publisher->Init()) {
+          _publishers.insert({actor, new_publisher});
+          publisher = new_publisher;
+        }
+        std::shared_ptr<CarlaTransformPublisher> new_transform = std::make_shared<CarlaTransformPublisher>(ros_name.c_str(), parent_ros_name.c_str());
+        if (new_transform->Init()) {
+          _transforms.insert({actor, new_transform});
+          transform = new_transform;
+        }
+      } break;
+      case ESensors::WorldObserver: {
+        std::cerr << "World obserser does not have an available publisher" << std::endl;
+      } break;
+      case ESensors::CameraGBufferUint8: {
+        std::cerr << "Camera GBuffer uint8 does not have an available publisher" << std::endl;
+      } break;
+      case ESensors::CameraGBufferFloat: {
+        std::cerr << "Camera GBuffer float does not have an available publisher" << std::endl;
+      } break;
+      default: {
+        std::cerr << "Unknown sensor type" << std::endl;
+      }
+    }
+  }
+  return { publisher, transform };
+}
+
+void ROS2::ProcessDataFromCamera(
+    uint64_t sensor_type,
+    carla::streaming::detail::stream_id_type stream_id,
+    const carla::geom::Transform sensor_transform,
+    int W, int H, float Fov,
+    const carla::SharedBufferView buffer,
+    void *actor) {
+
+  switch (sensor_type) {
+    case ESensors::CollisionSensor:
+      log_info("Sensor Collision to ROS data: frame.", _frame, "sensor.", sensor_type, "stream.", stream_id, "buffer.", buffer->size());
+      break;
+    case ESensors::DepthCamera:
+      {
+        log_info("Sensor DepthCamera to ROS data: frame.", _frame, "sensor.", sensor_type, "stream.", stream_id, "buffer.", buffer->size());
+        auto sensors = GetOrCreateSensor(ESensors::DepthCamera, stream_id, actor);
+        if (sensors.first) {
+          std::shared_ptr<CarlaDepthCameraPublisher> publisher = std::dynamic_pointer_cast<CarlaDepthCameraPublisher>(sensors.first);
+          const carla::sensor::s11n::ImageSerializer::ImageHeader *header =
+            reinterpret_cast<const carla::sensor::s11n::ImageSerializer::ImageHeader *>(buffer->data());
+          if (!header)
+            return;
+          if (!publisher->HasBeenInitialized())
+            publisher->InitInfoData(0, 0, H, W, Fov, true);
+          publisher->SetImageData(_seconds, _nanoseconds, header->height, header->width, (const uint8_t*) (buffer->data() + carla::sensor::s11n::ImageSerializer::header_offset));
+          publisher->SetCameraInfoData(_seconds, _nanoseconds);
+          publisher->Publish();
+        }
+        if (sensors.second) {
+          std::shared_ptr<CarlaTransformPublisher> publisher = std::dynamic_pointer_cast<CarlaTransformPublisher>(sensors.second);
+          publisher->SetData(_seconds, _nanoseconds, (const float*)&sensor_transform.location, (const float*)&sensor_transform.rotation);
+          publisher->Publish();
+        }
+      }
+      break;
+    case ESensors::NormalsCamera:
+      log_info("Sensor NormalsCamera to ROS data: frame.", _frame, "sensor.", sensor_type, "stream.", stream_id, "buffer.", buffer->size());
+      {
+        auto sensors = GetOrCreateSensor(ESensors::NormalsCamera, stream_id, actor);
+        if (sensors.first) {
+          std::shared_ptr<CarlaNormalsCameraPublisher> publisher = std::dynamic_pointer_cast<CarlaNormalsCameraPublisher>(sensors.first);
+          const carla::sensor::s11n::ImageSerializer::ImageHeader *header =
+            reinterpret_cast<const carla::sensor::s11n::ImageSerializer::ImageHeader *>(buffer->data());
+          if (!header)
+            return;
+          if (!publisher->HasBeenInitialized())
+            publisher->InitInfoData(0, 0, H, W, Fov, true);
+          publisher->SetImageData(_seconds, _nanoseconds, header->height, header->width, (const uint8_t*) (buffer->data() + carla::sensor::s11n::ImageSerializer::header_offset));
+          publisher->SetCameraInfoData(_seconds, _nanoseconds);
+          publisher->Publish();
+        }
+        if (sensors.second) {
+          std::shared_ptr<CarlaTransformPublisher> publisher = std::dynamic_pointer_cast<CarlaTransformPublisher>(sensors.second);
+          publisher->SetData(_seconds, _nanoseconds, (const float*)&sensor_transform.location, (const float*)&sensor_transform.rotation);
+          publisher->Publish();
+        }
+      }
+      break;
+    case ESensors::LaneInvasionSensor:
+      log_info("Sensor LaneInvasionSensor to ROS data: frame.", _frame, "sensor.", sensor_type, "stream.", stream_id, "buffer.", buffer->size());
+      {
+        auto sensors = GetOrCreateSensor(ESensors::LaneInvasionSensor, stream_id, actor);
+        if (sensors.first) {
+          std::shared_ptr<CarlaLineInvasionPublisher> publisher = std::dynamic_pointer_cast<CarlaLineInvasionPublisher>(sensors.first);
+          publisher->SetData(_seconds, _nanoseconds, (const int32_t*) buffer->data());
+          publisher->Publish();
+        }
+        if (sensors.second) {
+          std::shared_ptr<CarlaTransformPublisher> publisher = std::dynamic_pointer_cast<CarlaTransformPublisher>(sensors.second);
+          publisher->SetData(_seconds, _nanoseconds, (const float*)&sensor_transform.location, (const float*)&sensor_transform.rotation);
+          publisher->Publish();
+        }
+      }
+      break;
+    case ESensors::OpticalFlowCamera:
+      log_info("Sensor OpticalFlowCamera to ROS data: frame.", _frame, "sensor.", sensor_type, "stream.", stream_id, "buffer.", buffer->size());
+      {
+        auto sensors = GetOrCreateSensor(ESensors::OpticalFlowCamera, stream_id, actor);
+        if (sensors.first) {
+          std::shared_ptr<CarlaOpticalFlowCameraPublisher> publisher = std::dynamic_pointer_cast<CarlaOpticalFlowCameraPublisher>(sensors.first);
+          const carla::sensor::s11n::OpticalFlowImageSerializer::ImageHeader *header =
+            reinterpret_cast<const carla::sensor::s11n::OpticalFlowImageSerializer::ImageHeader *>(buffer->data());
+          if (!header)
+            return;
+          if (!publisher->HasBeenInitialized())
+            publisher->InitInfoData(0, 0, H, W, Fov, true);
+          publisher->SetImageData(_seconds, _nanoseconds, header->height, header->width, (const float*) (buffer->data() + carla::sensor::s11n::OpticalFlowImageSerializer::header_offset));
+          publisher->SetCameraInfoData(_seconds, _nanoseconds);
+          publisher->Publish();
+        }
+        if (sensors.second) {
+          std::shared_ptr<CarlaTransformPublisher> publisher = std::dynamic_pointer_cast<CarlaTransformPublisher>(sensors.second);
+          publisher->SetData(_seconds, _nanoseconds, (const float*)&sensor_transform.location, (const float*)&sensor_transform.rotation);
+          publisher->Publish();
+        }
+      }
+      break;
+    case ESensors::RssSensor:
+      log_info("Sensor RssSensor to ROS data: frame.", _frame, "sensor.", sensor_type, "stream.", stream_id, "buffer.", buffer->size());
+      break;
+    case ESensors::SceneCaptureCamera:
+    {
+      log_info("Sensor SceneCaptureCamera to ROS data: frame.", _frame, "sensor.", sensor_type, "stream.", stream_id, "buffer.", buffer->size());
+      {
+        auto sensors = GetOrCreateSensor(ESensors::SceneCaptureCamera, stream_id, actor);
+        if (sensors.first) {
+          std::shared_ptr<CarlaRGBCameraPublisher> publisher = std::dynamic_pointer_cast<CarlaRGBCameraPublisher>(sensors.first);
+          const carla::sensor::s11n::ImageSerializer::ImageHeader *header =
+            reinterpret_cast<const carla::sensor::s11n::ImageSerializer::ImageHeader *>(buffer->data());
+          if (!header)
+            return;
+          if (!publisher->HasBeenInitialized())
+            publisher->InitInfoData(0, 0, H, W, Fov, true);
+          publisher->SetImageData(_seconds, _nanoseconds, header->height, header->width, (const uint8_t*) (buffer->data() + carla::sensor::s11n::ImageSerializer::header_offset));
+          publisher->SetCameraInfoData(_seconds, _nanoseconds);
+          publisher->Publish();
+        }
+        if (sensors.second) {
+          std::shared_ptr<CarlaTransformPublisher> publisher = std::dynamic_pointer_cast<CarlaTransformPublisher>(sensors.second);
+          publisher->SetData(_seconds, _nanoseconds, (const float*)&sensor_transform.location, (const float*)&sensor_transform.rotation);
+          publisher->Publish();
+        }
+      }
+      break;
+    }
+    case ESensors::SemanticSegmentationCamera:
+      log_info("Sensor SemanticSegmentationCamera to ROS data: frame.", _frame, "sensor.", sensor_type, "stream.", stream_id, "buffer.", buffer->size());
+      {
+        auto sensors = GetOrCreateSensor(ESensors::SemanticSegmentationCamera, stream_id, actor);
+        if (sensors.first) {
+          std::shared_ptr<CarlaSSCameraPublisher> publisher = std::dynamic_pointer_cast<CarlaSSCameraPublisher>(sensors.first);
+          const carla::sensor::s11n::ImageSerializer::ImageHeader *header =
+            reinterpret_cast<const carla::sensor::s11n::ImageSerializer::ImageHeader *>(buffer->data());
+          if (!header)
+            return;
+          if (!publisher->HasBeenInitialized())
+            publisher->InitInfoData(0, 0, H, W, Fov, true);
+          publisher->SetImageData(_seconds, _nanoseconds, header->height, header->width, (const uint8_t*) (buffer->data() + carla::sensor::s11n::ImageSerializer::header_offset));
+          publisher->SetCameraInfoData(_seconds, _nanoseconds);
+          publisher->Publish();
+        }
+        if (sensors.second) {
+          std::shared_ptr<CarlaTransformPublisher> publisher = std::dynamic_pointer_cast<CarlaTransformPublisher>(sensors.second);
+          publisher->SetData(_seconds, _nanoseconds, (const float*)&sensor_transform.location, (const float*)&sensor_transform.rotation);
+          publisher->Publish();
+        }
+      }
+      break;
+    case ESensors::InstanceSegmentationCamera:
+      log_info("Sensor InstanceSegmentationCamera to ROS data: frame.", _frame, "sensor.", sensor_type, "stream.", stream_id, "buffer.", buffer->size());
+      {
+        auto sensors = GetOrCreateSensor(ESensors::InstanceSegmentationCamera, stream_id, actor);
+        if (sensors.first) {
+          std::shared_ptr<CarlaISCameraPublisher> publisher = std::dynamic_pointer_cast<CarlaISCameraPublisher>(sensors.first);
+          const carla::sensor::s11n::ImageSerializer::ImageHeader *header =
+            reinterpret_cast<const carla::sensor::s11n::ImageSerializer::ImageHeader *>(buffer->data());
+          if (!header)
+            return;
+          if (!publisher->HasBeenInitialized())
+            publisher->InitInfoData(0, 0, H, W, Fov, true);
+          publisher->SetImageData(_seconds, _nanoseconds, header->height, header->width, (const uint8_t*) (buffer->data() + carla::sensor::s11n::ImageSerializer::header_offset));
+          publisher->SetCameraInfoData(_seconds, _nanoseconds);
+          publisher->Publish();
+        }
+        if (sensors.second) {
+          std::shared_ptr<CarlaTransformPublisher> publisher = std::dynamic_pointer_cast<CarlaTransformPublisher>(sensors.second);
+          publisher->SetData(_seconds, _nanoseconds, (const float*)&sensor_transform.location, (const float*)&sensor_transform.rotation);
+          publisher->Publish();
+        }
+      }
+      break;
+    case ESensors::WorldObserver:
+      log_info("Sensor WorldObserver to ROS data: frame.", _frame, "sensor.", sensor_type, "stream.", stream_id, "buffer.", buffer->size());
+      break;
+    case ESensors::CameraGBufferUint8:
+      log_info("Sensor CameraGBufferUint8 to ROS data: frame.", _frame, "sensor.", sensor_type, "stream.", stream_id, "buffer.", buffer->size());
+      break;
+    case ESensors::CameraGBufferFloat:
+      log_info("Sensor CameraGBufferFloat to ROS data: frame.", _frame, "sensor.", sensor_type, "stream.", stream_id, "buffer.", buffer->size());
+      break;
+    default:
+      log_info("Sensor to ROS data: frame.", _frame, "sensor.", sensor_type, "stream.", stream_id, "buffer.", buffer->size());
+  }
+}
+
+void ROS2::ProcessDataFromGNSS(
+    uint64_t sensor_type,
+    carla::streaming::detail::stream_id_type stream_id,
+    const carla::geom::Transform sensor_transform,
+    const carla::geom::GeoLocation &data,
+    void *actor) {
+  log_info("Sensor GnssSensor to ROS data: frame.", _frame, "sensor.", sensor_type, "stream.", stream_id, "geo.", data.latitude, data.longitude, data.altitude);
+  auto sensors = GetOrCreateSensor(ESensors::GnssSensor, stream_id, actor);
+  if (sensors.first) {
+    std::shared_ptr<CarlaGNSSPublisher> publisher = std::dynamic_pointer_cast<CarlaGNSSPublisher>(sensors.first);
+    publisher->SetData(_seconds, _nanoseconds, reinterpret_cast<const double*>(&data));
+    publisher->Publish();
+  }
+  if (sensors.second) {
+    std::shared_ptr<CarlaTransformPublisher> publisher = std::dynamic_pointer_cast<CarlaTransformPublisher>(sensors.second);
+    publisher->SetData(_seconds, _nanoseconds, (const float*)&sensor_transform.location, (const float*)&sensor_transform.rotation);
+    publisher->Publish();
+  }
+}
+
+void ROS2::ProcessDataFromIMU(
+    uint64_t sensor_type,
+    carla::streaming::detail::stream_id_type stream_id,
+    const carla::geom::Transform sensor_transform,
+    carla::geom::Vector3D accelerometer,
+    carla::geom::Vector3D gyroscope,
+    float compass,
+    void *actor) {
+  log_info("Sensor InertialMeasurementUnit to ROS data: frame.", _frame, "sensor.", sensor_type, "stream.", stream_id, "imu.", accelerometer.x, gyroscope.x, compass);
+  auto sensors = GetOrCreateSensor(ESensors::InertialMeasurementUnit, stream_id, actor);
+  if (sensors.first) {
+    std::shared_ptr<CarlaIMUPublisher> publisher = std::dynamic_pointer_cast<CarlaIMUPublisher>(sensors.first);
+    publisher->SetData(_seconds, _nanoseconds, reinterpret_cast<float*>(&accelerometer), reinterpret_cast<float*>(&gyroscope), compass);
+    publisher->Publish();
+  }
+  if (sensors.second) {
+    std::shared_ptr<CarlaTransformPublisher> publisher = std::dynamic_pointer_cast<CarlaTransformPublisher>(sensors.second);
+    publisher->SetData(_seconds, _nanoseconds, (const float*)&sensor_transform.location, (const float*)&sensor_transform.rotation);
+    publisher->Publish();
+  }
+}
+
+void ROS2::ProcessDataFromDVS(
+    uint64_t sensor_type,
+    carla::streaming::detail::stream_id_type stream_id,
+    const carla::geom::Transform sensor_transform,
+    const carla::SharedBufferView buffer,
+    int W, int H, float Fov,
+    void *actor) {
+  log_info("Sensor DVS to ROS data: frame.", _frame, "sensor.", sensor_type, "stream.", stream_id);
+  auto sensors = GetOrCreateSensor(ESensors::DVSCamera, stream_id, actor);
+  if (sensors.first) {
+    std::shared_ptr<CarlaDVSCameraPublisher> publisher = std::dynamic_pointer_cast<CarlaDVSCameraPublisher>(sensors.first);
+    const carla::sensor::s11n::ImageSerializer::ImageHeader *header =
+      reinterpret_cast<const carla::sensor::s11n::ImageSerializer::ImageHeader *>(buffer->data());
+    if (!header)
+      return;
+    if (!publisher->HasBeenInitialized())
+      publisher->InitInfoData(0, 0, H, W, Fov, true);
+    size_t elements = (buffer->size() - carla::sensor::s11n::ImageSerializer::header_offset) / sizeof(carla::sensor::data::DVSEvent);
+    publisher->SetImageData(_seconds, _nanoseconds, elements, header->height, header->width, (const uint8_t*) (buffer->data() + carla::sensor::s11n::ImageSerializer::header_offset));
+    publisher->SetCameraInfoData(_seconds, _nanoseconds);
+    publisher->SetPointCloudData(1, elements * sizeof(carla::sensor::data::DVSEvent), elements, (const uint8_t*) (buffer->data() + carla::sensor::s11n::ImageSerializer::header_offset));
+    publisher->Publish();
+  }
+  if (sensors.second) {
+    std::shared_ptr<CarlaTransformPublisher> publisher = std::dynamic_pointer_cast<CarlaTransformPublisher>(sensors.second);
+    publisher->SetData(_seconds, _nanoseconds, (const float*)&sensor_transform.location, (const float*)&sensor_transform.rotation);
+    publisher->Publish();
+  }
+}
+
+void ROS2::ProcessDataFromLidar(
+    uint64_t sensor_type,
+    carla::streaming::detail::stream_id_type stream_id,
+    const carla::geom::Transform sensor_transform,
+    carla::sensor::data::LidarData &data,
+    void *actor) {
+  log_info("Sensor Lidar to ROS data: frame.", _frame, "sensor.", sensor_type, "stream.", stream_id, "points.", data._points.size());
+  auto sensors = GetOrCreateSensor(ESensors::RayCastLidar, stream_id, actor);
+  if (sensors.first) {
+    std::shared_ptr<CarlaLidarPublisher> publisher = std::dynamic_pointer_cast<CarlaLidarPublisher>(sensors.first);
+    size_t width = data._points.size();
+    size_t height = 1;
+    publisher->SetData(_seconds, _nanoseconds, height, width, (float*)data._points.data());
+    publisher->Publish();
+  }
+  if (sensors.second) {
+    std::shared_ptr<CarlaTransformPublisher> publisher = std::dynamic_pointer_cast<CarlaTransformPublisher>(sensors.second);
+    publisher->SetData(_seconds, _nanoseconds, (const float*)&sensor_transform.location, (const float*)&sensor_transform.rotation);
+    publisher->Publish();
+  }
+}
+
+void ROS2::ProcessDataFromSemanticLidar(
+    uint64_t sensor_type,
+    carla::streaming::detail::stream_id_type stream_id,
+    const carla::geom::Transform sensor_transform,
+    carla::sensor::data::SemanticLidarData &data,
+    void *actor) {
+  static_assert(sizeof(float) == sizeof(uint32_t), "Invalid float size");
+  log_info("Sensor SemanticLidar to ROS data: frame.", _frame, "sensor.", sensor_type, "stream.", stream_id, "points.", data._ser_points.size());
+  auto sensors = GetOrCreateSensor(ESensors::RayCastSemanticLidar, stream_id, actor);
+  if (sensors.first) {
+    std::shared_ptr<CarlaSemanticLidarPublisher> publisher = std::dynamic_pointer_cast<CarlaSemanticLidarPublisher>(sensors.first);
+    size_t width = data._ser_points.size();
+    size_t height = 1;
+    publisher->SetData(_seconds, _nanoseconds, 6, height, width, (float*)data._ser_points.data());
+    publisher->Publish();
+  }
+  if (sensors.second) {
+    std::shared_ptr<CarlaTransformPublisher> publisher = std::dynamic_pointer_cast<CarlaTransformPublisher>(sensors.second);
+    publisher->SetData(_seconds, _nanoseconds, (const float*)&sensor_transform.location, (const float*)&sensor_transform.rotation);
+    publisher->Publish();
+  }
+}
+
+void ROS2::ProcessDataFromRadar(
+    uint64_t sensor_type,
+    carla::streaming::detail::stream_id_type stream_id,
+    const carla::geom::Transform sensor_transform,
+    const carla::sensor::data::RadarData &data,
+    void *actor) {
+  log_info("Sensor Radar to ROS data: frame.", _frame, "sensor.", sensor_type, "stream.", stream_id, "points.", data._detections.size());
+  auto sensors = GetOrCreateSensor(ESensors::Radar, stream_id, actor);
+  if (sensors.first) {
+    std::shared_ptr<CarlaRadarPublisher> publisher = std::dynamic_pointer_cast<CarlaRadarPublisher>(sensors.first);
+    size_t elements = data.GetDetectionCount();
+    size_t width = elements * sizeof(carla::sensor::data::RadarDetection);
+    size_t height = 1;
+    publisher->SetData(_seconds, _nanoseconds, height, width, elements, (const uint8_t*)data._detections.data());
+    publisher->Publish();
+  }
+  if (sensors.second) {
+    std::shared_ptr<CarlaTransformPublisher> publisher = std::dynamic_pointer_cast<CarlaTransformPublisher>(sensors.second);
+    publisher->SetData(_seconds, _nanoseconds, (const float*)&sensor_transform.location, (const float*)&sensor_transform.rotation);
+    publisher->Publish();
+  }
+}
+
+void ROS2::ProcessDataFromObstacleDetection(
+    uint64_t sensor_type,
+    carla::streaming::detail::stream_id_type stream_id,
+    const carla::geom::Transform sensor_transform,
+    AActor *first_ctor,
+    AActor *second_actor,
+    float distance,
+    void *actor) {
+  log_info("Sensor ObstacleDetector to ROS data: frame.", _frame, "sensor.", sensor_type, "stream.", stream_id, "distance.", distance);
+}
+
+void ROS2::ProcessDataFromCollisionSensor(
+    uint64_t sensor_type,
+    carla::streaming::detail::stream_id_type stream_id,
+    const carla::geom::Transform sensor_transform,
+    uint32_t other_actor,
+    carla::geom::Vector3D impulse,
+    void* actor) {
+  auto sensors = GetOrCreateSensor(ESensors::CollisionSensor, stream_id, actor);
+  if (sensors.first) {
+    std::shared_ptr<CarlaCollisionPublisher> publisher = std::dynamic_pointer_cast<CarlaCollisionPublisher>(sensors.first);
+    publisher->SetData(_seconds, _nanoseconds, other_actor, impulse.x, impulse.y, impulse.z);
+    publisher->Publish();
+  }
+  if (sensors.second) {
+    std::shared_ptr<CarlaTransformPublisher> publisher = std::dynamic_pointer_cast<CarlaTransformPublisher>(sensors.second);
+    publisher->SetData(_seconds, _nanoseconds, (const float*)&sensor_transform.location, (const float*)&sensor_transform.rotation);
+    publisher->Publish();
+  }
+}
+
+void ROS2::Shutdown() {
+  for (auto& element : _publishers) {
+    element.second.reset();
+  }
+  for (auto& element : _transforms) {
+    element.second.reset();
+  }
+  _clock_publisher.reset();
+  _controller.reset();
+  _enabled = false;
+}
+
+} // namespace ros2
+} // namespace carla

LibCarla/source/carla/ros2/ROS2.h

@@ -0,0 +1,167 @@
+// Copyright (c) 2023 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 <https://opensource.org/licenses/MIT>.
+
+#pragma once
+
+#include "carla/Buffer.h"
+#include "carla/BufferView.h"
+#include "carla/geom/Transform.h"
+#include "carla/ros2/ROS2CallbackData.h"
+#include "carla/streaming/detail/Types.h"
+
+#include <unordered_set>
+#include <unordered_map>
+#include <memory>
+#include <vector>
+
+// forward declarations
+class AActor;
+namespace carla {
+  namespace geom {
+    class GeoLocation;
+    class Vector3D;
+  }
+  namespace sensor {
+    namespace data {
+      struct DVSEvent;
+      class LidarData;
+      class SemanticLidarData;
+      class RadarData;
+    }
+  }
+}
+
+namespace carla {
+namespace ros2 {
+
+  class CarlaPublisher;
+  class CarlaTransformPublisher;
+  class CarlaClockPublisher;
+  class CarlaEgoVehicleControlSubscriber;
+
+class ROS2
+{
+  public:
+
+  // deleting copy constructor for singleton
+  ROS2(const ROS2& obj) = delete;
+  static std::shared_ptr<ROS2> GetInstance() {
+    if (!_instance)
+      _instance = std::shared_ptr<ROS2>(new ROS2);
+    return _instance;
+  }
+
+  // general
+  void Enable(bool enable);
+  void Shutdown();
+  bool IsEnabled() { return _enabled; }
+  void SetFrame(uint64_t frame);
+  void SetTimestamp(double timestamp);
+
+  // ros_name managing
+  void AddActorRosName(void *actor, std::string ros_name);
+  void AddActorParentRosName(void *actor, void* parent);
+  void RemoveActorRosName(void *actor);
+  void UpdateActorRosName(void *actor, std::string ros_name);
+  std::string GetActorRosName(void *actor);
+  std::string GetActorParentRosName(void *actor);
+
+  // callbacks
+  void AddActorCallback(void* actor, std::string ros_name, ActorCallback callback);
+  void RemoveActorCallback(void* actor);
+
+  // enabling streams to publish
+  void EnableStream(carla::streaming::detail::stream_id_type id) { _publish_stream.insert(id); }
+  bool IsStreamEnabled(carla::streaming::detail::stream_id_type id) { return _publish_stream.count(id) > 0; }
+  void ResetStreams() { _publish_stream.clear(); }
+
+  // receiving data to publish
+  void ProcessDataFromCamera(
+      uint64_t sensor_type,
+      carla::streaming::detail::stream_id_type stream_id,
+      const carla::geom::Transform sensor_transform,
+      int W, int H, float Fov,
+      const carla::SharedBufferView buffer,
+      void *actor = nullptr);
+  void ProcessDataFromGNSS(
+      uint64_t sensor_type,
+      carla::streaming::detail::stream_id_type stream_id,
+      const carla::geom::Transform sensor_transform,
+      const carla::geom::GeoLocation &data,
+      void *actor = nullptr);
+  void ProcessDataFromIMU(
+      uint64_t sensor_type,
+      carla::streaming::detail::stream_id_type stream_id,
+      const carla::geom::Transform sensor_transform,
+      carla::geom::Vector3D accelerometer,
+      carla::geom::Vector3D gyroscope,
+      float compass,
+      void *actor = nullptr);
+  void ProcessDataFromDVS(
+      uint64_t sensor_type,
+      carla::streaming::detail::stream_id_type stream_id,
+      const carla::geom::Transform sensor_transform,
+      const carla::SharedBufferView buffer,
+      int W, int H, float Fov,
+      void *actor = nullptr);
+  void ProcessDataFromLidar(
+      uint64_t sensor_type,
+      carla::streaming::detail::stream_id_type stream_id,
+      const carla::geom::Transform sensor_transform,
+      carla::sensor::data::LidarData &data,
+      void *actor = nullptr);
+  void ProcessDataFromSemanticLidar(
+      uint64_t sensor_type,
+      carla::streaming::detail::stream_id_type stream_id,
+      const carla::geom::Transform sensor_transform,
+      carla::sensor::data::SemanticLidarData &data,
+      void *actor = nullptr);
+  void ProcessDataFromRadar(
+      uint64_t sensor_type,
+      carla::streaming::detail::stream_id_type stream_id,
+      const carla::geom::Transform sensor_transform,
+      const carla::sensor::data::RadarData &data,
+      void *actor = nullptr);
+  void ProcessDataFromObstacleDetection(
+      uint64_t sensor_type,
+      carla::streaming::detail::stream_id_type stream_id,
+      const carla::geom::Transform sensor_transform,
+      AActor *first_actor,
+      AActor *second_actor,
+      float distance,
+      void *actor = nullptr);
+void ProcessDataFromCollisionSensor(
+    uint64_t sensor_type,
+    carla::streaming::detail::stream_id_type stream_id,
+    const carla::geom::Transform sensor_transform,
+    uint32_t other_actor,
+    carla::geom::Vector3D impulse,
+    void* actor);
+
+  private:
+  std::pair<std::shared_ptr<CarlaPublisher>, std::shared_ptr<CarlaTransformPublisher>> GetOrCreateSensor(int type, carla::streaming::detail::stream_id_type id, void* actor);
+
+  // sigleton
+  ROS2() {};
+
+  static std::shared_ptr<ROS2> _instance;
+
+  bool _enabled { false };
+  uint64_t _frame { 0 };
+  int32_t _seconds { 0 };
+  uint32_t _nanoseconds { 0 };
+  std::unordered_map<void *, std::string> _actor_ros_name;
+  std::unordered_map<void *, std::vector<void*> > _actor_parent_ros_name;
+  std::shared_ptr<CarlaEgoVehicleControlSubscriber> _controller;
+  std::shared_ptr<CarlaClockPublisher> _clock_publisher;
+  std::unordered_map<void *, std::shared_ptr<CarlaPublisher>> _publishers;
+  std::unordered_map<void *, std::shared_ptr<CarlaTransformPublisher>> _transforms;
+  std::unordered_set<carla::streaming::detail::stream_id_type> _publish_stream;
+  std::unordered_map<void *, ActorCallback> _actor_callbacks;
+};
+
+} // namespace ros2
+} // namespace carla

LibCarla/source/carla/ros2/ROS2CallbackData.h

@@ -0,0 +1,38 @@
+// Copyright (c) 2023 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 <https://opensource.org/licenses/MIT>.
+
+#pragma once
+
+#ifdef _MSC_VER
+#pragma warning(push)
+#pragma warning(disable:4583)
+#pragma warning(disable:4582)
+#include <boost/variant2/variant.hpp>
+#pragma warning(pop)
+#else
+#include <boost/variant2/variant.hpp>
+#endif
+
+namespace carla {
+namespace ros2 {
+
+  struct VehicleControl
+  {
+    float   throttle;
+    float   steer;
+    float   brake;
+    bool    hand_brake;
+    bool    reverse;
+    int32_t gear;
+    bool    manual_gear_shift;
+  };
+
+  using ROS2CallbackData = boost::variant2::variant<VehicleControl>;
+
+  using ActorCallback = std::function<void(void *actor, ROS2CallbackData data)>;
+
+} // namespace ros2
+} // namespace carla

LibCarla/source/carla/ros2/listeners/CarlaListener.cpp

@@ -0,0 +1,43 @@
+#define _GLIBCXX_USE_CXX11_ABI 0
+
+#include "CarlaListener.h"
+#include <iostream>
+
+#include <fastdds/dds/publisher/DataWriterListener.hpp>
+#include <fastdds/dds/core/status/PublicationMatchedStatus.hpp>
+
+namespace carla {
+namespace ros2 {
+
+  namespace efd = eprosima::fastdds::dds;
+
+    class CarlaListenerImpl : public efd::DataWriterListener {
+      public:
+      void on_publication_matched(
+              efd::DataWriter* writer,
+              const efd::PublicationMatchedStatus& info) override;
+
+
+      int _matched {0};
+      bool _first_connected {false};
+    };
+
+    void CarlaListenerImpl::on_publication_matched(efd::DataWriter* writer, const efd::PublicationMatchedStatus& info)
+    {
+      if (info.current_count_change == 1) {
+          _matched = info.total_count;
+          _first_connected = true;
+      } else if (info.current_count_change == -1) {
+          _matched = info.total_count;
+      } else {
+          std::cerr << info.current_count_change
+                  << " is not a valid value for PublicationMatchedStatus current count change" << std::endl;
+      }
+    }
+
+    CarlaListener::CarlaListener() :
+    _impl(std::make_unique<CarlaListenerImpl>()) { }
+
+    CarlaListener::~CarlaListener() {}
+
+}}

LibCarla/source/carla/ros2/listeners/CarlaListener.h

@@ -0,0 +1,27 @@
+// Copyright (c) 2022 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 <https://opensource.org/licenses/MIT>.
+
+#pragma once
+#define _GLIBCXX_USE_CXX11_ABI 0
+
+#include <memory>
+
+namespace carla {
+namespace ros2 {
+
+  class CarlaListenerImpl;
+
+  class CarlaListener {
+    public:
+      CarlaListener();
+      ~CarlaListener();
+      CarlaListener(const CarlaListener&) = delete;
+      CarlaListener& operator=(const CarlaListener&) = delete;
+      CarlaListener(CarlaListener&&) = delete;
+      CarlaListener& operator=(CarlaListener&&) = delete;
+
+      std::unique_ptr<CarlaListenerImpl> _impl;
+  };
+}
+}

LibCarla/source/carla/ros2/listeners/CarlaSubscriberListener.cpp

@@ -0,0 +1,116 @@
+#define _GLIBCXX_USE_CXX11_ABI 0
+
+#include "CarlaSubscriberListener.h"
+#include <iostream>
+
+#include <fastdds/dds/subscriber/DataReader.hpp>
+#include <fastdds/dds/subscriber/DataReaderListener.hpp>
+#include <fastdds/dds/core/status/SubscriptionMatchedStatus.hpp>
+#include <fastdds/dds/subscriber/SampleInfo.hpp>
+#include "carla/ros2/types/CarlaEgoVehicleControl.h"
+#include "carla/ros2/subscribers/CarlaEgoVehicleControlSubscriber.h"
+#include "carla/ros2/ROS2CallbackData.h"
+
+namespace carla {
+namespace ros2 {
+
+  namespace efd = eprosima::fastdds::dds;
+  using erc = eprosima::fastrtps::types::ReturnCode_t;
+
+    class CarlaSubscriberListenerImpl : public efd::DataReaderListener {
+      public:
+      void on_subscription_matched(
+              efd::DataReader* reader,
+              const efd::SubscriptionMatchedStatus& info) override;
+      void on_data_available(efd::DataReader* reader) override;
+
+      int _matched {0};
+      bool _first_connected {false};
+      CarlaEgoVehicleControlSubscriber* _owner {nullptr};
+      carla_msgs::msg::CarlaEgoVehicleControl _message {};
+    };
+
+    void CarlaSubscriberListenerImpl::on_subscription_matched(efd::DataReader* reader, const efd::SubscriptionMatchedStatus& info)
+    {
+      if (info.current_count_change == 1) {
+          _matched = info.total_count;
+          _first_connected = true;
+      } else if (info.current_count_change == -1) {
+          _matched = info.total_count;
+          if (_matched == 0) {
+            _owner->DestroySubscriber();
+          }
+      } else {
+          std::cerr << info.current_count_change
+                  << " is not a valid value for PublicationMatchedStatus current count change" << std::endl;
+      }
+    }
+
+    void CarlaSubscriberListenerImpl::on_data_available(efd::DataReader* reader)
+    {
+      efd::SampleInfo info;
+      eprosima::fastrtps::types::ReturnCode_t rcode = reader->take_next_sample(&_message, &info);
+      if (rcode == erc::ReturnCodeValue::RETCODE_OK) {
+        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();
+        _owner->ForwardMessage(control);
+      }
+      if (rcode == erc::ReturnCodeValue::RETCODE_ERROR) {
+          std::cerr << "RETCODE_ERROR" << std::endl;
+      }
+      if (rcode == erc::ReturnCodeValue::RETCODE_UNSUPPORTED) {
+          std::cerr << "RETCODE_UNSUPPORTED" << std::endl;
+      }
+      if (rcode == erc::ReturnCodeValue::RETCODE_BAD_PARAMETER) {
+          std::cerr << "RETCODE_BAD_PARAMETER" << std::endl;
+      }
+      if (rcode == erc::ReturnCodeValue::RETCODE_PRECONDITION_NOT_MET) {
+          std::cerr << "RETCODE_PRECONDITION_NOT_MET" << std::endl;
+      }
+      if (rcode == erc::ReturnCodeValue::RETCODE_OUT_OF_RESOURCES) {
+          std::cerr << "RETCODE_OUT_OF_RESOURCES" << std::endl;
+      }
+      if (rcode == erc::ReturnCodeValue::RETCODE_NOT_ENABLED) {
+          std::cerr << "RETCODE_NOT_ENABLED" << std::endl;
+      }
+      if (rcode == erc::ReturnCodeValue::RETCODE_IMMUTABLE_POLICY) {
+          std::cerr << "RETCODE_IMMUTABLE_POLICY" << std::endl;
+      }
+      if (rcode == erc::ReturnCodeValue::RETCODE_INCONSISTENT_POLICY) {
+          std::cerr << "RETCODE_INCONSISTENT_POLICY" << std::endl;
+      }
+      if (rcode == erc::ReturnCodeValue::RETCODE_ALREADY_DELETED) {
+          std::cerr << "RETCODE_ALREADY_DELETED" << std::endl;
+      }
+      if (rcode == erc::ReturnCodeValue::RETCODE_TIMEOUT) {
+          std::cerr << "RETCODE_TIMEOUT" << std::endl;
+      }
+      if (rcode == erc::ReturnCodeValue::RETCODE_NO_DATA) {
+          std::cerr << "RETCODE_NO_DATA" << std::endl;
+      }
+      if (rcode == erc::ReturnCodeValue::RETCODE_ILLEGAL_OPERATION) {
+          std::cerr << "RETCODE_ILLEGAL_OPERATION" << std::endl;
+      }
+      if (rcode == erc::ReturnCodeValue::RETCODE_NOT_ALLOWED_BY_SECURITY) {
+          std::cerr << "RETCODE_NOT_ALLOWED_BY_SECURITY" << std::endl;
+      }
+    }
+
+    void CarlaSubscriberListener::SetOwner(CarlaEgoVehicleControlSubscriber* owner) {
+        _impl->_owner = owner;
+    }
+
+    CarlaSubscriberListener::CarlaSubscriberListener(CarlaEgoVehicleControlSubscriber* owner) :
+    _impl(std::make_unique<CarlaSubscriberListenerImpl>()) {
+        _impl->_owner = owner;
+    }
+
+    CarlaSubscriberListener::~CarlaSubscriberListener() {}
+
+}}

LibCarla/source/carla/ros2/listeners/CarlaSubscriberListener.h

@@ -0,0 +1,30 @@
+// Copyright (c) 2022 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 <https://opensource.org/licenses/MIT>.
+
+#pragma once
+#define _GLIBCXX_USE_CXX11_ABI 0
+
+#include <memory>
+
+namespace carla {
+namespace ros2 {
+
+  class CarlaSubscriberListenerImpl;
+  class CarlaEgoVehicleControlSubscriber;
+
+  class CarlaSubscriberListener {
+    public:
+      CarlaSubscriberListener(CarlaEgoVehicleControlSubscriber* owner);
+      ~CarlaSubscriberListener();
+      CarlaSubscriberListener(const CarlaSubscriberListener&) = delete;
+      CarlaSubscriberListener& operator=(const CarlaSubscriberListener&) = delete;
+      CarlaSubscriberListener(CarlaSubscriberListener&&) = delete;
+      CarlaSubscriberListener& operator=(CarlaSubscriberListener&&) = delete;
+
+      void SetOwner(CarlaEgoVehicleControlSubscriber* owner);
+
+      std::unique_ptr<CarlaSubscriberListenerImpl> _impl;
+  };
+}
+}

LibCarla/source/carla/ros2/publishers/CarlaClockPublisher.cpp

@@ -0,0 +1,205 @@
+#define _GLIBCXX_USE_CXX11_ABI 0
+
+#include "CarlaClockPublisher.h"
+
+#include <string>
+
+#include "carla/ros2/types/TimePubSubTypes.h"
+#include "carla/ros2/listeners/CarlaListener.h"
+
+#include <fastdds/dds/domain/DomainParticipant.hpp>
+#include <fastdds/dds/publisher/Publisher.hpp>
+#include <fastdds/dds/topic/Topic.hpp>
+#include <fastdds/dds/publisher/DataWriter.hpp>
+#include <fastdds/dds/topic/TypeSupport.hpp>
+
+#include <fastdds/dds/domain/qos/DomainParticipantQos.hpp>
+#include <fastdds/dds/domain/DomainParticipantFactory.hpp>
+#include <fastdds/dds/publisher/qos/PublisherQos.hpp>
+#include <fastdds/dds/topic/qos/TopicQos.hpp>
+
+#include <fastrtps/attributes/ParticipantAttributes.h>
+#include <fastrtps/qos/QosPolicies.h>
+#include <fastdds/dds/publisher/qos/DataWriterQos.hpp>
+#include <fastdds/dds/publisher/DataWriterListener.hpp>
+
+
+namespace carla {
+namespace ros2 {
+  namespace efd = eprosima::fastdds::dds;
+  using erc = eprosima::fastrtps::types::ReturnCode_t;
+
+  struct CarlaClockPublisherImpl {
+    efd::DomainParticipant* _participant { nullptr };
+    efd::Publisher* _publisher { nullptr };
+    efd::Topic* _topic { nullptr };
+    efd::DataWriter* _datawriter { nullptr };
+    efd::TypeSupport _type { new builtin_interfaces::msg::TimePubSubType() };
+    CarlaListener _listener {};
+    builtin_interfaces::msg::Time _time {};
+  };
+
+  bool CarlaClockPublisher::Init() {
+    if (_impl->_type == nullptr) {
+        std::cerr << "Invalid TypeSupport" << std::endl;
+        return false;
+    }
+    efd::DomainParticipantQos pqos = efd::PARTICIPANT_QOS_DEFAULT;
+    pqos.name(_name);
+    auto factory = efd::DomainParticipantFactory::get_instance();
+    _impl->_participant = factory->create_participant(0, pqos);
+    if (_impl->_participant == nullptr) {
+        std::cerr << "Failed to create DomainParticipant" << std::endl;
+        return false;
+    }
+    _impl->_type.register_type(_impl->_participant);
+
+    efd::PublisherQos pubqos = efd::PUBLISHER_QOS_DEFAULT;
+    _impl->_publisher = _impl->_participant->create_publisher(pubqos, nullptr);
+    if (_impl->_publisher == nullptr) {
+      std::cerr << "Failed to create Publisher" << std::endl;
+      return false;
+    }
+
+    efd::TopicQos tqos = efd::TOPIC_QOS_DEFAULT;
+    const std::string topic_name { "rt/clock" };
+    _impl->_topic = _impl->_participant->create_topic(topic_name, _impl->_type->getName(), tqos);
+    if (_impl->_topic == nullptr) {
+        std::cerr << "Failed to create Topic" << std::endl;
+        return false;
+    }
+
+    efd::DataWriterQos wqos = efd::DATAWRITER_QOS_DEFAULT;
+    efd::DataWriterListener* listener = (efd::DataWriterListener*)_impl->_listener._impl.get();
+    _impl->_datawriter = _impl->_publisher->create_datawriter(_impl->_topic, wqos, listener);
+    if (_impl->_datawriter == nullptr) {
+        std::cerr << "Failed to create DataWriter" << std::endl;
+        return false;
+    }
+    _frame_id = _name;
+    return true;
+  }
+
+  bool CarlaClockPublisher::Publish() {
+    eprosima::fastrtps::rtps::InstanceHandle_t instance_handle;
+    eprosima::fastrtps::types::ReturnCode_t rcode = _impl->_datawriter->write(&_impl->_time, instance_handle);
+    if (rcode == erc::ReturnCodeValue::RETCODE_OK) {
+        return true;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_ERROR) {
+        std::cerr << "RETCODE_ERROR" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_UNSUPPORTED) {
+        std::cerr << "RETCODE_UNSUPPORTED" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_BAD_PARAMETER) {
+        std::cerr << "RETCODE_BAD_PARAMETER" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_PRECONDITION_NOT_MET) {
+        std::cerr << "RETCODE_PRECONDITION_NOT_MET" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_OUT_OF_RESOURCES) {
+        std::cerr << "RETCODE_OUT_OF_RESOURCES" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_NOT_ENABLED) {
+        std::cerr << "RETCODE_NOT_ENABLED" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_IMMUTABLE_POLICY) {
+        std::cerr << "RETCODE_IMMUTABLE_POLICY" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_INCONSISTENT_POLICY) {
+        std::cerr << "RETCODE_INCONSISTENT_POLICY" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_ALREADY_DELETED) {
+        std::cerr << "RETCODE_ALREADY_DELETED" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_TIMEOUT) {
+        std::cerr << "RETCODE_TIMEOUT" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_NO_DATA) {
+        std::cerr << "RETCODE_NO_DATA" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_ILLEGAL_OPERATION) {
+        std::cerr << "RETCODE_ILLEGAL_OPERATION" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_NOT_ALLOWED_BY_SECURITY) {
+        std::cerr << "RETCODE_NOT_ALLOWED_BY_SECURITY" << std::endl;
+        return false;
+    }
+    std::cerr << "UNKNOWN" << std::endl;
+    return false;
+  }
+
+  void CarlaClockPublisher::SetData(int32_t sec, uint32_t nanosec) {
+    _impl->_time.sec(sec);
+    _impl->_time.nanosec(nanosec);
+  }
+
+  CarlaClockPublisher::CarlaClockPublisher(const char* ros_name, const char* parent) :
+  _impl(std::make_shared<CarlaClockPublisherImpl>()) {
+    _name = ros_name;
+    _parent = parent;
+  }
+
+  CarlaClockPublisher::~CarlaClockPublisher() {
+      if (!_impl)
+          return;
+
+      if (_impl->_datawriter)
+          _impl->_publisher->delete_datawriter(_impl->_datawriter);
+
+      if (_impl->_publisher)
+          _impl->_participant->delete_publisher(_impl->_publisher);
+
+      if (_impl->_topic)
+          _impl->_participant->delete_topic(_impl->_topic);
+
+      if (_impl->_participant)
+          efd::DomainParticipantFactory::get_instance()->delete_participant(_impl->_participant);
+  }
+
+  CarlaClockPublisher::CarlaClockPublisher(const CarlaClockPublisher& other) {
+    _frame_id = other._frame_id;
+    _name = other._name;
+    _parent = other._parent;
+    _impl = other._impl;
+  }
+
+  CarlaClockPublisher& CarlaClockPublisher::operator=(const CarlaClockPublisher& other) {
+    _frame_id = other._frame_id;
+    _name = other._name;
+    _parent = other._parent;
+    _impl = other._impl;
+
+    return *this;
+  }
+
+  CarlaClockPublisher::CarlaClockPublisher(CarlaClockPublisher&& other) {
+    _frame_id = std::move(other._frame_id);
+    _name = std::move(other._name);
+    _parent = std::move(other._parent);
+    _impl = std::move(other._impl);
+  }
+
+  CarlaClockPublisher& CarlaClockPublisher::operator=(CarlaClockPublisher&& other) {
+    _frame_id = std::move(other._frame_id);
+    _name = std::move(other._name);
+    _parent = std::move(other._parent);
+    _impl = std::move(other._impl);
+
+    return *this;
+  }
+}
+}

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

@@ -0,0 +1,36 @@
+// Copyright (c) 2022 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 <https://opensource.org/licenses/MIT>.
+
+#pragma once
+#define _GLIBCXX_USE_CXX11_ABI 0
+
+#include <memory>
+#include <vector>
+
+#include "CarlaPublisher.h"
+
+namespace carla {
+namespace ros2 {
+
+  struct CarlaClockPublisherImpl;
+
+  class CarlaClockPublisher : public CarlaPublisher {
+    public:
+      CarlaClockPublisher(const char* ros_name = "", const char* parent = "");
+      ~CarlaClockPublisher();
+      CarlaClockPublisher(const CarlaClockPublisher&);
+      CarlaClockPublisher& operator=(const CarlaClockPublisher&);
+      CarlaClockPublisher(CarlaClockPublisher&&);
+      CarlaClockPublisher& operator=(CarlaClockPublisher&&);
+
+      bool Init();
+      bool Publish();
+      void SetData(int32_t sec, uint32_t nanosec);
+      const char* type() const override { return "clock"; }
+
+    private:
+      std::shared_ptr<CarlaClockPublisherImpl> _impl;
+  };
+}
+}

LibCarla/source/carla/ros2/publishers/CarlaCollisionPublisher.cpp

@@ -0,0 +1,231 @@
+#define _GLIBCXX_USE_CXX11_ABI 0
+
+#include "CarlaCollisionPublisher.h"
+
+#include <string>
+
+#include "carla/ros2/types/CarlaCollisionEventPubSubTypes.h"
+#include "carla/ros2/listeners/CarlaListener.h"
+
+#include <fastdds/dds/domain/DomainParticipant.hpp>
+#include <fastdds/dds/publisher/Publisher.hpp>
+#include <fastdds/dds/topic/Topic.hpp>
+#include <fastdds/dds/publisher/DataWriter.hpp>
+#include <fastdds/dds/topic/TypeSupport.hpp>
+
+#include <fastdds/dds/domain/qos/DomainParticipantQos.hpp>
+#include <fastdds/dds/domain/DomainParticipantFactory.hpp>
+#include <fastdds/dds/publisher/qos/PublisherQos.hpp>
+#include <fastdds/dds/topic/qos/TopicQos.hpp>
+
+#include <fastrtps/attributes/ParticipantAttributes.h>
+#include <fastrtps/qos/QosPolicies.h>
+#include <fastdds/dds/publisher/qos/DataWriterQos.hpp>
+#include <fastdds/dds/publisher/DataWriterListener.hpp>
+
+
+namespace carla {
+namespace ros2 {
+
+  namespace efd = eprosima::fastdds::dds;
+  using erc = eprosima::fastrtps::types::ReturnCode_t;
+
+  struct CarlaCollisionPublisherImpl {
+    efd::DomainParticipant* _participant { nullptr };
+    efd::Publisher* _publisher { nullptr };
+    efd::Topic* _topic { nullptr };
+    efd::DataWriter* _datawriter { nullptr };
+    efd::TypeSupport _type { new carla_msgs::msg::CollisionEventPubSubType() };
+    CarlaListener _listener {};
+    carla_msgs::msg::CollisionEvent _event {};
+  };
+
+  bool CarlaCollisionPublisher::Init() {
+    if (_impl->_type == nullptr) {
+        std::cerr << "Invalid TypeSupport" << std::endl;
+        return false;
+    }
+
+    efd::DomainParticipantQos pqos = efd::PARTICIPANT_QOS_DEFAULT;
+    pqos.name(_name);
+    auto factory = efd::DomainParticipantFactory::get_instance();
+    _impl->_participant = factory->create_participant(0, pqos);
+    if (_impl->_participant == nullptr) {
+        std::cerr << "Failed to create DomainParticipant" << std::endl;
+        return false;
+    }
+    _impl->_type.register_type(_impl->_participant);
+
+    efd::PublisherQos pubqos = efd::PUBLISHER_QOS_DEFAULT;
+    _impl->_publisher = _impl->_participant->create_publisher(pubqos, nullptr);
+    if (_impl->_publisher == nullptr) {
+      std::cerr << "Failed to create Publisher" << std::endl;
+      return false;
+    }
+
+    efd::TopicQos tqos = efd::TOPIC_QOS_DEFAULT;
+    const std::string base { "rt/carla/" };
+    std::string topic_name = base;
+    if (!_parent.empty())
+      topic_name += _parent + "/";
+    topic_name += _name;
+    _impl->_topic = _impl->_participant->create_topic(topic_name, _impl->_type->getName(), tqos);
+    if (_impl->_topic == nullptr) {
+        std::cerr << "Failed to create Topic" << std::endl;
+        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 = (efd::DataWriterListener*)_impl->_listener._impl.get();
+    _impl->_datawriter = _impl->_publisher->create_datawriter(_impl->_topic, wqos, listener);
+    if (_impl->_datawriter == nullptr) {
+        std::cerr << "Failed to create DataWriter" << std::endl;
+        return false;
+    }
+
+    _frame_id = _name;
+    return true;
+  }
+
+  bool CarlaCollisionPublisher::Publish() {
+    eprosima::fastrtps::rtps::InstanceHandle_t instance_handle;
+    eprosima::fastrtps::types::ReturnCode_t rcode = _impl->_datawriter->write(&_impl->_event, instance_handle);
+    if (rcode == erc::ReturnCodeValue::RETCODE_OK) {
+        return true;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_ERROR) {
+        std::cerr << "RETCODE_ERROR" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_UNSUPPORTED) {
+        std::cerr << "RETCODE_UNSUPPORTED" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_BAD_PARAMETER) {
+        std::cerr << "RETCODE_BAD_PARAMETER" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_PRECONDITION_NOT_MET) {
+        std::cerr << "RETCODE_PRECONDITION_NOT_MET" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_OUT_OF_RESOURCES) {
+        std::cerr << "RETCODE_OUT_OF_RESOURCES" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_NOT_ENABLED) {
+        std::cerr << "RETCODE_NOT_ENABLED" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_IMMUTABLE_POLICY) {
+        std::cerr << "RETCODE_IMMUTABLE_POLICY" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_INCONSISTENT_POLICY) {
+        std::cerr << "RETCODE_INCONSISTENT_POLICY" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_ALREADY_DELETED) {
+        std::cerr << "RETCODE_ALREADY_DELETED" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_TIMEOUT) {
+        std::cerr << "RETCODE_TIMEOUT" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_NO_DATA) {
+        std::cerr << "RETCODE_NO_DATA" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_ILLEGAL_OPERATION) {
+        std::cerr << "RETCODE_ILLEGAL_OPERATION" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_NOT_ALLOWED_BY_SECURITY) {
+        std::cerr << "RETCODE_NOT_ALLOWED_BY_SECURITY" << std::endl;
+        return false;
+    }
+    std::cerr << "UNKNOWN" << std::endl;
+    return false;
+  }
+
+void CarlaCollisionPublisher::SetData(int32_t seconds, uint32_t nanoseconds, uint32_t actor_id, float x, float y, float z) {
+    std::vector<float> vector_data ;
+    SetData(seconds, nanoseconds, actor_id, {x, y, z});
+  }
+
+  void CarlaCollisionPublisher::SetData(int32_t seconds, uint32_t nanoseconds, uint32_t actor_id, std::vector<float>&& data) {
+    builtin_interfaces::msg::Time time;
+    time.sec(seconds);
+    time.nanosec(nanoseconds);
+
+    std_msgs::msg::Header header;
+    header.stamp(std::move(time));
+    header.frame_id(_frame_id);
+
+    geometry_msgs::msg::Vector3 impulse;
+    impulse.x(data[0]);
+    impulse.y(data[1]);
+    impulse.z(data[2]);
+    _impl->_event.header(std::move(header));
+    _impl->_event.other_actor_id(actor_id);
+    _impl->_event.normal_impulse(impulse);
+  }
+
+  CarlaCollisionPublisher::CarlaCollisionPublisher(const char* ros_name, const char* parent) :
+  _impl(std::make_shared<CarlaCollisionPublisherImpl>()) {
+    _name = ros_name;
+    _parent = parent;
+  }
+
+  CarlaCollisionPublisher::~CarlaCollisionPublisher() {
+      if (!_impl)
+          return;
+
+      if (_impl->_datawriter)
+          _impl->_publisher->delete_datawriter(_impl->_datawriter);
+
+      if (_impl->_publisher)
+          _impl->_participant->delete_publisher(_impl->_publisher);
+
+      if (_impl->_topic)
+          _impl->_participant->delete_topic(_impl->_topic);
+
+      if (_impl->_participant)
+          efd::DomainParticipantFactory::get_instance()->delete_participant(_impl->_participant);
+  }
+
+  CarlaCollisionPublisher::CarlaCollisionPublisher(const CarlaCollisionPublisher& other) {
+    _frame_id = other._frame_id;
+    _name = other._name;
+    _parent = other._parent;
+    _impl = other._impl;
+  }
+
+  CarlaCollisionPublisher& CarlaCollisionPublisher::operator=(const CarlaCollisionPublisher& other) {
+    _frame_id = other._frame_id;
+    _name = other._name;
+    _parent = other._parent;
+    _impl = other._impl;
+
+    return *this;
+  }
+
+  CarlaCollisionPublisher::CarlaCollisionPublisher(CarlaCollisionPublisher&& other) {
+    _frame_id = std::move(other._frame_id);
+    _name = std::move(other._name);
+    _parent = std::move(other._parent);
+    _impl = std::move(other._impl);
+  }
+
+  CarlaCollisionPublisher& CarlaCollisionPublisher::operator=(CarlaCollisionPublisher&& other) {
+    _frame_id = std::move(other._frame_id);
+    _name = std::move(other._name);
+    _parent = std::move(other._parent);
+    _impl = std::move(other._impl);
+
+    return *this;
+  }
+}
+}

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

@@ -0,0 +1,39 @@
+// Copyright (c) 2022 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 <https://opensource.org/licenses/MIT>.
+
+#pragma once
+#define _GLIBCXX_USE_CXX11_ABI 0
+
+#include <memory>
+#include <vector>
+
+#include "CarlaPublisher.h"
+
+namespace carla {
+namespace ros2 {
+
+  struct CarlaCollisionPublisherImpl;
+
+  class CarlaCollisionPublisher : public CarlaPublisher {
+    public:
+      CarlaCollisionPublisher(const char* ros_name = "", const char* parent = "");
+      ~CarlaCollisionPublisher();
+      CarlaCollisionPublisher(const CarlaCollisionPublisher&);
+      CarlaCollisionPublisher& operator=(const CarlaCollisionPublisher&);
+      CarlaCollisionPublisher(CarlaCollisionPublisher&&);
+      CarlaCollisionPublisher& operator=(CarlaCollisionPublisher&&);
+
+      bool Init();
+      bool Publish();
+      void SetData(int32_t seconds, uint32_t nanoseconds, uint32_t actor_id, float x, float y, float z);
+      const char* type() const override { return "collision"; }
+
+    private:
+      void SetData(int32_t seconds, uint32_t nanoseconds, uint32_t actor_id, std::vector<float>&& data);
+
+    private:
+      std::shared_ptr<CarlaCollisionPublisherImpl> _impl;
+  };
+}
+}

LibCarla/source/carla/ros2/publishers/CarlaDVSCameraPublisher.cpp

@@ -0,0 +1,605 @@
+#define _GLIBCXX_USE_CXX11_ABI 0
+
+#include "CarlaDVSCameraPublisher.h"
+
+#include <string>
+
+#include "carla/sensor/data/DVSEvent.h"
+
+#include "carla/ros2/types/ImagePubSubTypes.h"
+#include "carla/ros2/types/CameraInfoPubSubTypes.h"
+#include "carla/ros2/types/PointCloud2PubSubTypes.h"
+#include "carla/ros2/listeners/CarlaListener.h"
+
+#include <fastdds/dds/domain/DomainParticipant.hpp>
+#include <fastdds/dds/publisher/Publisher.hpp>
+#include <fastdds/dds/topic/Topic.hpp>
+#include <fastdds/dds/publisher/DataWriter.hpp>
+#include <fastdds/dds/topic/TypeSupport.hpp>
+
+#include <fastdds/dds/domain/qos/DomainParticipantQos.hpp>
+#include <fastdds/dds/domain/DomainParticipantFactory.hpp>
+#include <fastdds/dds/publisher/qos/PublisherQos.hpp>
+#include <fastdds/dds/topic/qos/TopicQos.hpp>
+
+#include <fastrtps/attributes/ParticipantAttributes.h>
+#include <fastrtps/qos/QosPolicies.h>
+#include <fastdds/dds/publisher/qos/DataWriterQos.hpp>
+#include <fastdds/dds/publisher/DataWriterListener.hpp>
+
+
+namespace carla {
+namespace ros2 {
+
+  namespace efd = eprosima::fastdds::dds;
+  using erc = eprosima::fastrtps::types::ReturnCode_t;
+
+  struct CarlaDVSCameraPublisherImpl {
+    efd::DomainParticipant* _participant { nullptr };
+    efd::Publisher* _publisher { nullptr };
+    efd::Topic* _topic { nullptr };
+    efd::DataWriter* _datawriter { nullptr };
+    efd::TypeSupport _type { new sensor_msgs::msg::ImagePubSubType() };
+    CarlaListener _listener {};
+    sensor_msgs::msg::Image _image {};
+  };
+
+  struct CarlaCameraInfoPublisherImpl {
+    efd::DomainParticipant* _participant { nullptr };
+    efd::Publisher* _publisher { nullptr };
+    efd::Topic* _topic { nullptr };
+    efd::DataWriter* _datawriter { nullptr };
+    efd::TypeSupport _type { new sensor_msgs::msg::CameraInfoPubSubType() };
+    CarlaListener _listener {};
+    bool _init {false};
+    sensor_msgs::msg::CameraInfo _ci {};
+  };
+
+  struct CarlaPointCloudPublisherImpl {
+    efd::DomainParticipant* _participant { nullptr };
+    efd::Publisher* _publisher { nullptr };
+    efd::Topic* _topic { nullptr };
+    efd::DataWriter* _datawriter { nullptr };
+    efd::TypeSupport _type { new sensor_msgs::msg::PointCloud2PubSubType() };
+    CarlaListener _listener {};
+    sensor_msgs::msg::PointCloud2 _pc {};
+  };
+
+  bool CarlaDVSCameraPublisher::HasBeenInitialized() const {
+    return _info->_init;
+  }
+
+  void CarlaDVSCameraPublisher::InitInfoData(uint32_t x_offset, uint32_t y_offset, uint32_t height, uint32_t width, float fov, bool do_rectify) {
+    _info->_ci = std::move(sensor_msgs::msg::CameraInfo(height, width, fov));
+    SetInfoRegionOfInterest(x_offset, y_offset, height, width, do_rectify);
+    _info->_init = true;
+  }
+
+  bool CarlaDVSCameraPublisher::Init() {
+    return InitImage() && InitInfo() && InitPointCloud();
+  }
+
+  bool CarlaDVSCameraPublisher::InitImage() {
+    if (_impl->_type == nullptr) {
+        std::cerr << "Invalid TypeSupport" << std::endl;
+        return false;
+    }
+
+    efd::DomainParticipantQos pqos = efd::PARTICIPANT_QOS_DEFAULT;
+    pqos.name(_name);
+    auto factory = efd::DomainParticipantFactory::get_instance();
+    _impl->_participant = factory->create_participant(0, pqos);
+    if (_impl->_participant == nullptr) {
+        std::cerr << "Failed to create DomainParticipant" << std::endl;
+        return false;
+    }
+    _impl->_type.register_type(_impl->_participant);
+
+    efd::PublisherQos pubqos = efd::PUBLISHER_QOS_DEFAULT;
+    _impl->_publisher = _impl->_participant->create_publisher(pubqos, nullptr);
+    if (_impl->_publisher == nullptr) {
+      std::cerr << "Failed to create Publisher" << std::endl;
+      return false;
+    }
+
+    efd::TopicQos tqos = efd::TOPIC_QOS_DEFAULT;
+    const std::string publisher_type {"/image"};
+    const std::string base { "rt/carla/" };
+    std::string topic_name = base;
+    if (!_parent.empty())
+      topic_name += _parent + "/";
+    topic_name += _name;
+    topic_name += publisher_type;
+    _impl->_topic = _impl->_participant->create_topic(topic_name, _impl->_type->getName(), tqos);
+    if (_impl->_topic == nullptr) {
+        std::cerr << "Failed to create Topic" << std::endl;
+        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 = (efd::DataWriterListener*)_impl->_listener._impl.get();
+    _impl->_datawriter = _impl->_publisher->create_datawriter(_impl->_topic, wqos, listener);
+    if (_impl->_datawriter == nullptr) {
+        std::cerr << "Failed to create DataWriter" << std::endl;
+        return false;
+    }
+    _frame_id = _name;
+    return true;
+  }
+
+  bool CarlaDVSCameraPublisher::InitInfo() {
+    if (_info->_type == nullptr) {
+        std::cerr << "Invalid TypeSupport" << std::endl;
+        return false;
+    }
+
+    efd::DomainParticipantQos pqos = efd::PARTICIPANT_QOS_DEFAULT;
+    pqos.name(_name);
+    auto factory = efd::DomainParticipantFactory::get_instance();
+    _info->_participant = factory->create_participant(0, pqos);
+    if (_info->_participant == nullptr) {
+        std::cerr << "Failed to create DomainParticipant" << std::endl;
+        return false;
+    }
+    _info->_type.register_type(_info->_participant);
+
+    efd::PublisherQos pubqos = efd::PUBLISHER_QOS_DEFAULT;
+    _info->_publisher = _info->_participant->create_publisher(pubqos, nullptr);
+    if (_info->_publisher == nullptr) {
+      std::cerr << "Failed to create Publisher" << std::endl;
+      return false;
+    }
+
+    efd::TopicQos tqos = efd::TOPIC_QOS_DEFAULT;
+    const std::string publisher_type {"/camera_info"};
+    const std::string base { "rt/carla/" };
+    std::string topic_name = base;
+    if (!_parent.empty())
+      topic_name += _parent + "/";
+    topic_name += _name;
+    topic_name += publisher_type;
+    _info->_topic = _info->_participant->create_topic(topic_name, _info->_type->getName(), tqos);
+    if (_info->_topic == nullptr) {
+        std::cerr << "Failed to create Topic" << std::endl;
+        return false;
+    }
+    efd::DataWriterQos wqos = efd::DATAWRITER_QOS_DEFAULT;
+    efd::DataWriterListener* listener = (efd::DataWriterListener*)_info->_listener._impl.get();
+    _info->_datawriter = _info->_publisher->create_datawriter(_info->_topic, wqos, listener);
+    if (_info->_datawriter == nullptr) {
+        std::cerr << "Failed to create DataWriter" << std::endl;
+        return false;
+    }
+
+    _frame_id = _name;
+    return true;
+  }
+
+  bool CarlaDVSCameraPublisher::InitPointCloud() {
+    if (_point_cloud->_type == nullptr) {
+        std::cerr << "Invalid TypeSupport" << std::endl;
+        return false;
+    }
+
+    efd::DomainParticipantQos pqos = efd::PARTICIPANT_QOS_DEFAULT;
+    pqos.name(_name);
+    auto factory = efd::DomainParticipantFactory::get_instance();
+    _point_cloud->_participant = factory->create_participant(0, pqos);
+    if (_point_cloud->_participant == nullptr) {
+        std::cerr << "Failed to create DomainParticipant" << std::endl;
+        return false;
+    }
+    _point_cloud->_type.register_type(_point_cloud->_participant);
+
+    efd::PublisherQos pubqos = efd::PUBLISHER_QOS_DEFAULT;
+    _point_cloud->_publisher = _point_cloud->_participant->create_publisher(pubqos, nullptr);
+    if (_point_cloud->_publisher == nullptr) {
+      std::cerr << "Failed to create Publisher" << std::endl;
+      return false;
+    }
+
+    efd::TopicQos tqos = efd::TOPIC_QOS_DEFAULT;
+    const std::string publisher_type {"/point_cloud"};
+    const std::string base { "rt/carla/" };
+    std::string topic_name = base;
+    if (!_parent.empty())
+      topic_name += _parent + "/";
+    topic_name += _name;
+    topic_name += publisher_type;
+    _point_cloud->_topic = _point_cloud->_participant->create_topic(topic_name, _point_cloud->_type->getName(), tqos);
+    if (_point_cloud->_topic == nullptr) {
+        std::cerr << "Failed to create Topic" << std::endl;
+        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 = (efd::DataWriterListener*)_point_cloud->_listener._impl.get();
+    _point_cloud->_datawriter = _point_cloud->_publisher->create_datawriter(_point_cloud->_topic, wqos, listener);
+    if (_point_cloud->_datawriter == nullptr) {
+        std::cerr << "Failed to create DataWriter" << std::endl;
+        return false;
+    }
+    _frame_id = _name;
+    return true;
+  }
+
+  bool CarlaDVSCameraPublisher::Publish() {
+    return PublishImage() && PublishInfo() && PublishPointCloud();
+  }
+
+  bool CarlaDVSCameraPublisher::PublishImage() {
+    eprosima::fastrtps::rtps::InstanceHandle_t instance_handle;
+    eprosima::fastrtps::types::ReturnCode_t rcode = _impl->_datawriter->write(& _impl->_image, instance_handle);
+    if (rcode == erc::ReturnCodeValue::RETCODE_OK) {
+        return true;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_ERROR) {
+        std::cerr << "RETCODE_ERROR" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_UNSUPPORTED) {
+        std::cerr << "RETCODE_UNSUPPORTED" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_BAD_PARAMETER) {
+        std::cerr << "RETCODE_BAD_PARAMETER" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_PRECONDITION_NOT_MET) {
+        std::cerr << "RETCODE_PRECONDITION_NOT_MET" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_OUT_OF_RESOURCES) {
+        std::cerr << "RETCODE_OUT_OF_RESOURCES" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_NOT_ENABLED) {
+        std::cerr << "RETCODE_NOT_ENABLED" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_IMMUTABLE_POLICY) {
+        std::cerr << "RETCODE_IMMUTABLE_POLICY" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_INCONSISTENT_POLICY) {
+        std::cerr << "RETCODE_INCONSISTENT_POLICY" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_ALREADY_DELETED) {
+        std::cerr << "RETCODE_ALREADY_DELETED" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_TIMEOUT) {
+        std::cerr << "RETCODE_TIMEOUT" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_NO_DATA) {
+        std::cerr << "RETCODE_NO_DATA" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_ILLEGAL_OPERATION) {
+        std::cerr << "RETCODE_ILLEGAL_OPERATION" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_NOT_ALLOWED_BY_SECURITY) {
+        std::cerr << "RETCODE_NOT_ALLOWED_BY_SECURITY" << std::endl;
+        return false;
+    }
+    std::cerr << "UNKNOWN" << std::endl;
+    return false;
+  }
+
+  bool CarlaDVSCameraPublisher::PublishInfo() {
+    eprosima::fastrtps::rtps::InstanceHandle_t instance_handle;
+    eprosima::fastrtps::types::ReturnCode_t rcode = _info->_datawriter->write(& _info->_ci, instance_handle);
+    if (rcode == erc::ReturnCodeValue::RETCODE_OK) {
+        return true;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_ERROR) {
+        std::cerr << "RETCODE_ERROR" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_UNSUPPORTED) {
+        std::cerr << "RETCODE_UNSUPPORTED" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_BAD_PARAMETER) {
+        std::cerr << "RETCODE_BAD_PARAMETER" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_PRECONDITION_NOT_MET) {
+        std::cerr << "RETCODE_PRECONDITION_NOT_MET" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_OUT_OF_RESOURCES) {
+        std::cerr << "RETCODE_OUT_OF_RESOURCES" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_NOT_ENABLED) {
+        std::cerr << "RETCODE_NOT_ENABLED" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_IMMUTABLE_POLICY) {
+        std::cerr << "RETCODE_IMMUTABLE_POLICY" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_INCONSISTENT_POLICY) {
+        std::cerr << "RETCODE_INCONSISTENT_POLICY" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_ALREADY_DELETED) {
+        std::cerr << "RETCODE_ALREADY_DELETED" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_TIMEOUT) {
+        std::cerr << "RETCODE_TIMEOUT" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_NO_DATA) {
+        std::cerr << "RETCODE_NO_DATA" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_ILLEGAL_OPERATION) {
+        std::cerr << "RETCODE_ILLEGAL_OPERATION" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_NOT_ALLOWED_BY_SECURITY) {
+        std::cerr << "RETCODE_NOT_ALLOWED_BY_SECURITY" << std::endl;
+        return false;
+    }
+    std::cerr << "UNKNOWN" << std::endl;
+    return false;
+  }
+
+  bool CarlaDVSCameraPublisher::PublishPointCloud() {
+    eprosima::fastrtps::rtps::InstanceHandle_t instance_handle;
+    eprosima::fastrtps::types::ReturnCode_t rcode = _point_cloud->_datawriter->write(&_point_cloud->_pc, instance_handle);
+    if (rcode == erc::ReturnCodeValue::RETCODE_OK) {
+        return true;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_ERROR) {
+        std::cerr << "RETCODE_ERROR" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_UNSUPPORTED) {
+        std::cerr << "RETCODE_UNSUPPORTED" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_BAD_PARAMETER) {
+        std::cerr << "RETCODE_BAD_PARAMETER" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_PRECONDITION_NOT_MET) {
+        std::cerr << "RETCODE_PRECONDITION_NOT_MET" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_OUT_OF_RESOURCES) {
+        std::cerr << "RETCODE_OUT_OF_RESOURCES" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_NOT_ENABLED) {
+        std::cerr << "RETCODE_NOT_ENABLED" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_IMMUTABLE_POLICY) {
+        std::cerr << "RETCODE_IMMUTABLE_POLICY" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_INCONSISTENT_POLICY) {
+        std::cerr << "RETCODE_INCONSISTENT_POLICY" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_ALREADY_DELETED) {
+        std::cerr << "RETCODE_ALREADY_DELETED" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_TIMEOUT) {
+        std::cerr << "RETCODE_TIMEOUT" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_NO_DATA) {
+        std::cerr << "RETCODE_NO_DATA" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_ILLEGAL_OPERATION) {
+        std::cerr << "RETCODE_ILLEGAL_OPERATION" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_NOT_ALLOWED_BY_SECURITY) {
+        std::cerr << "RETCODE_NOT_ALLOWED_BY_SECURITY" << std::endl;
+        return false;
+    }
+    std::cerr << "UNKNOWN" << std::endl;
+    return false;
+  }
+
+  void CarlaDVSCameraPublisher::SetImageData(int32_t seconds, uint32_t nanoseconds, size_t elements, size_t height, size_t width, const uint8_t* data) {
+    std::vector<uint8_t> im_data;
+    const size_t im_size = width * height * 3;
+    im_data.resize(im_size);
+    carla::sensor::data::DVSEvent* vec_event = (carla::sensor::data::DVSEvent*)&data[0];
+    for (size_t i = 0; i < elements; ++i, ++vec_event) {
+        size_t index = (vec_event->y * width + vec_event->x) * 3 + (static_cast<int>(vec_event->pol) * 2);
+        im_data[index] = 255;
+    }
+
+    SetData(seconds, nanoseconds, height, width, std::move(im_data));
+  }
+
+  void CarlaDVSCameraPublisher::SetData(int32_t seconds, uint32_t nanoseconds, size_t height, size_t width, std::vector<uint8_t>&& data) {
+    builtin_interfaces::msg::Time time;
+    time.sec(seconds);
+    time.nanosec(nanoseconds);
+
+    std_msgs::msg::Header header;
+    header.stamp(std::move(time));
+    header.frame_id(_frame_id);
+    _impl->_image.header(header);
+    _info->_ci.header(header);
+    _point_cloud->_pc.header(header);
+
+    _impl->_image.width(width);
+    _impl->_image.height(height);
+    _impl->_image.encoding("bgr8"); //taken from the list of strings in include/sensor_msgs/image_encodings.h
+    _impl->_image.is_bigendian(0);
+    _impl->_image.step(_impl->_image.width() * sizeof(uint8_t) * 3);
+    _impl->_image.data(std::move(data)); //https://github.com/eProsima/Fast-DDS/issues/2330
+  }
+
+  void CarlaDVSCameraPublisher::SetCameraInfoData(int32_t seconds, uint32_t nanoseconds) {
+    builtin_interfaces::msg::Time time;
+    time.sec(seconds);
+    time.nanosec(nanoseconds);
+
+    std_msgs::msg::Header header;
+    header.stamp(std::move(time));
+    header.frame_id(_frame_id);
+  }
+
+  void CarlaDVSCameraPublisher::SetInfoRegionOfInterest( uint32_t x_offset, uint32_t y_offset, uint32_t height, uint32_t width, bool do_rectify) {
+    sensor_msgs::msg::RegionOfInterest roi;
+    roi.x_offset(x_offset);
+    roi.y_offset(y_offset);
+    roi.height(height);
+    roi.width(width);
+    roi.do_rectify(do_rectify);
+    _info->_ci.roi(roi);
+  }
+
+  void CarlaDVSCameraPublisher::SetPointCloudData(size_t height, size_t width, size_t elements, const uint8_t* data) {
+
+    std::vector<uint8_t> vector_data;
+    const size_t size = height * width;
+    vector_data.resize(size);
+    std::memcpy(&vector_data[0], &data[0], size);
+
+    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);
+
+    const size_t point_size = sizeof(carla::sensor::data::DVSEvent);
+    _point_cloud->_pc.width(width);
+    _point_cloud->_pc.height(height);
+    _point_cloud->_pc.is_bigendian(false);
+    _point_cloud->_pc.fields({descriptor1, descriptor2, descriptor3, descriptor4});
+    _point_cloud->_pc.point_step(point_size);
+    _point_cloud->_pc.row_step(width * point_size);
+    _point_cloud->_pc.is_dense(false); //True if there are not invalid points
+    _point_cloud->_pc.data(std::move(vector_data));
+  }
+
+  CarlaDVSCameraPublisher::CarlaDVSCameraPublisher(const char* ros_name, const char* parent) :
+  _impl(std::make_shared<CarlaDVSCameraPublisherImpl>()),
+  _info(std::make_shared<CarlaCameraInfoPublisherImpl>()),
+  _point_cloud(std::make_shared<CarlaPointCloudPublisherImpl>()) {
+    _name = ros_name;
+    _parent = parent;
+  }
+
+  CarlaDVSCameraPublisher::~CarlaDVSCameraPublisher() {
+      if (!_impl)
+          return;
+
+      if (_impl->_datawriter)
+          _impl->_publisher->delete_datawriter(_impl->_datawriter);
+
+      if (_impl->_publisher)
+          _impl->_participant->delete_publisher(_impl->_publisher);
+
+      if (_impl->_topic)
+          _impl->_participant->delete_topic(_impl->_topic);
+
+      if (_impl->_participant)
+          efd::DomainParticipantFactory::get_instance()->delete_participant(_impl->_participant);
+
+      if (!_info)
+          return;
+
+      if (_info->_datawriter)
+          _info->_publisher->delete_datawriter(_info->_datawriter);
+
+      if (_info->_publisher)
+          _info->_participant->delete_publisher(_info->_publisher);
+
+      if (_info->_topic)
+          _info->_participant->delete_topic(_info->_topic);
+
+      if (_info->_participant)
+          efd::DomainParticipantFactory::get_instance()->delete_participant(_info->_participant);
+
+      if (!_point_cloud)
+          return;
+
+      if (_point_cloud->_datawriter)
+          _point_cloud->_publisher->delete_datawriter(_point_cloud->_datawriter);
+
+      if (_point_cloud->_publisher)
+          _point_cloud->_participant->delete_publisher(_point_cloud->_publisher);
+
+      if (_point_cloud->_topic)
+          _point_cloud->_participant->delete_topic(_point_cloud->_topic);
+
+      if (_point_cloud->_participant)
+          efd::DomainParticipantFactory::get_instance()->delete_participant(_point_cloud->_participant);
+  }
+
+  CarlaDVSCameraPublisher::CarlaDVSCameraPublisher(const CarlaDVSCameraPublisher& other) {
+    _frame_id = other._frame_id;
+    _name = other._name;
+    _parent = other._parent;
+    _impl = other._impl;
+    _info = other._info;
+    _point_cloud = other._point_cloud;
+  }
+
+  CarlaDVSCameraPublisher& CarlaDVSCameraPublisher::operator=(const CarlaDVSCameraPublisher& other) {
+    _frame_id = other._frame_id;
+    _name = other._name;
+    _parent = other._parent;
+    _impl = other._impl;
+    _info = other._info;
+    _point_cloud = other._point_cloud;
+
+    return *this;
+  }
+
+  CarlaDVSCameraPublisher::CarlaDVSCameraPublisher(CarlaDVSCameraPublisher&& other) {
+    _frame_id = std::move(other._frame_id);
+    _name = std::move(other._name);
+    _parent = std::move(other._parent);
+    _impl = std::move(other._impl);
+    _info = std::move(other._info);
+    _point_cloud = std::move(other._point_cloud);
+  }
+
+  CarlaDVSCameraPublisher& CarlaDVSCameraPublisher::operator=(CarlaDVSCameraPublisher&& other) {
+    _frame_id = std::move(other._frame_id);
+    _name = std::move(other._name);
+    _parent = std::move(other._parent);
+    _impl = std::move(other._impl);
+    _info = std::move(other._info);
+    _point_cloud = std::move(other._point_cloud);
+
+    return *this;
+  }
+}
+}

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

@@ -0,0 +1,58 @@
+// Copyright (c) 2022 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 <https://opensource.org/licenses/MIT>.
+
+#pragma once
+#define _GLIBCXX_USE_CXX11_ABI 0
+
+#include <memory>
+#include <vector>
+
+#include "CarlaPublisher.h"
+
+namespace carla {
+namespace ros2 {
+
+  struct CarlaDVSCameraPublisherImpl;
+  struct CarlaCameraInfoPublisherImpl;
+  struct CarlaPointCloudPublisherImpl;
+
+  class CarlaDVSCameraPublisher : public CarlaPublisher {
+    public:
+      CarlaDVSCameraPublisher(const char* ros_name = "", const char* parent = "");
+      ~CarlaDVSCameraPublisher();
+      CarlaDVSCameraPublisher(const CarlaDVSCameraPublisher&);
+      CarlaDVSCameraPublisher& operator=(const CarlaDVSCameraPublisher&);
+      CarlaDVSCameraPublisher(CarlaDVSCameraPublisher&&);
+      CarlaDVSCameraPublisher& operator=(CarlaDVSCameraPublisher&&);
+
+      bool Init();
+      void InitInfoData(uint32_t x_offset, uint32_t y_offset, uint32_t height, uint32_t width, float fov, bool do_rectify);
+      bool Publish();
+
+      bool HasBeenInitialized() const;
+      void SetImageData(int32_t seconds, uint32_t nanoseconds, size_t elements, size_t height, size_t width, const uint8_t* data);
+      void SetCameraInfoData(int32_t seconds, uint32_t nanoseconds);
+      void SetPointCloudData(size_t height, size_t width, size_t elements, const uint8_t* data);
+      const char* type() const override { return "dvs camera"; }
+
+    private:
+    private:
+      bool InitImage();
+      bool InitInfo();
+      bool InitPointCloud();
+
+      void SetInfoRegionOfInterest( uint32_t x_offset, uint32_t y_offset, uint32_t height, uint32_t width, bool do_rectify);
+      void SetData(int32_t seconds, uint32_t nanoseconds, size_t height, size_t width, std::vector<uint8_t>&& data);
+      void SetPointCloudData(size_t height, size_t width, std::vector<uint8_t>&& data);
+      bool PublishImage();
+      bool PublishInfo();
+      bool PublishPointCloud();
+
+    private:
+      std::shared_ptr<CarlaDVSCameraPublisherImpl> _impl;
+      std::shared_ptr<CarlaCameraInfoPublisherImpl> _info;
+      std::shared_ptr<CarlaPointCloudPublisherImpl> _point_cloud;
+  };
+}
+}

LibCarla/source/carla/ros2/publishers/CarlaDepthCameraPublisher.cpp

@@ -0,0 +1,416 @@
+#define _GLIBCXX_USE_CXX11_ABI 0
+
+#include "CarlaDepthCameraPublisher.h"
+
+#include <string>
+
+#include "carla/ros2/types/ImagePubSubTypes.h"
+#include "carla/ros2/types/CameraInfoPubSubTypes.h"
+#include "carla/ros2/listeners/CarlaListener.h"
+
+#include <fastdds/dds/domain/DomainParticipant.hpp>
+#include <fastdds/dds/publisher/Publisher.hpp>
+#include <fastdds/dds/topic/Topic.hpp>
+#include <fastdds/dds/publisher/DataWriter.hpp>
+#include <fastdds/dds/topic/TypeSupport.hpp>
+
+#include <fastdds/dds/domain/qos/DomainParticipantQos.hpp>
+#include <fastdds/dds/domain/DomainParticipantFactory.hpp>
+#include <fastdds/dds/publisher/qos/PublisherQos.hpp>
+#include <fastdds/dds/topic/qos/TopicQos.hpp>
+
+#include <fastrtps/attributes/ParticipantAttributes.h>
+#include <fastrtps/qos/QosPolicies.h>
+#include <fastdds/dds/publisher/qos/DataWriterQos.hpp>
+#include <fastdds/dds/publisher/DataWriterListener.hpp>
+
+
+namespace carla {
+namespace ros2 {
+
+  namespace efd = eprosima::fastdds::dds;
+  using erc = eprosima::fastrtps::types::ReturnCode_t;
+
+  struct CarlaDepthCameraPublisherImpl {
+    efd::DomainParticipant* _participant { nullptr };
+    efd::Publisher* _publisher { nullptr };
+    efd::Topic* _topic { nullptr };
+    efd::DataWriter* _datawriter { nullptr };
+    efd::TypeSupport _type { new sensor_msgs::msg::ImagePubSubType() };
+    CarlaListener _listener {};
+    sensor_msgs::msg::Image _image {};
+  };
+
+  struct CarlaCameraInfoPublisherImpl {
+    efd::DomainParticipant* _participant { nullptr };
+    efd::Publisher* _publisher { nullptr };
+    efd::Topic* _topic { nullptr };
+    efd::DataWriter* _datawriter { nullptr };
+    efd::TypeSupport _type { new sensor_msgs::msg::CameraInfoPubSubType() };
+    CarlaListener _listener {};
+    bool _init {false};
+    sensor_msgs::msg::CameraInfo _info {};
+  };
+
+  bool CarlaDepthCameraPublisher::HasBeenInitialized() const {
+    return _impl_info->_init;
+  }
+
+  void CarlaDepthCameraPublisher::InitInfoData(uint32_t x_offset, uint32_t y_offset, uint32_t height, uint32_t width, float fov, bool do_rectify) {
+    _impl_info->_info = std::move(sensor_msgs::msg::CameraInfo(height, width, fov));
+    SetInfoRegionOfInterest(x_offset, y_offset, height, width, do_rectify);
+    _impl_info->_init = true;
+  }
+
+  bool CarlaDepthCameraPublisher::Init() {
+    return InitImage() && InitInfo();
+  }
+
+  bool CarlaDepthCameraPublisher::InitImage() {
+    if (_impl->_type == nullptr) {
+        std::cerr << "Invalid TypeSupport" << std::endl;
+        return false;
+    }
+
+    efd::DomainParticipantQos pqos = efd::PARTICIPANT_QOS_DEFAULT;
+    pqos.name(_name);
+    auto factory = efd::DomainParticipantFactory::get_instance();
+    _impl->_participant = factory->create_participant(0, pqos);
+    if (_impl->_participant == nullptr) {
+        std::cerr << "Failed to create DomainParticipant" << std::endl;
+        return false;
+    }
+    _impl->_type.register_type(_impl->_participant);
+
+    efd::PublisherQos pubqos = efd::PUBLISHER_QOS_DEFAULT;
+    _impl->_publisher = _impl->_participant->create_publisher(pubqos, nullptr);
+    if (_impl->_publisher == nullptr) {
+      std::cerr << "Failed to create Publisher" << std::endl;
+      return false;
+    }
+
+    efd::TopicQos tqos = efd::TOPIC_QOS_DEFAULT;
+    const std::string publisher_type {"/image"};
+    const std::string base { "rt/carla/" };
+    std::string topic_name = base;
+    if (!_parent.empty())
+      topic_name += _parent + "/";
+    topic_name += _name;
+    topic_name += publisher_type;
+    _impl->_topic = _impl->_participant->create_topic(topic_name, _impl->_type->getName(), tqos);
+    if (_impl->_topic == nullptr) {
+        std::cerr << "Failed to create Topic" << std::endl;
+        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 = (efd::DataWriterListener*)_impl->_listener._impl.get();
+    _impl->_datawriter = _impl->_publisher->create_datawriter(_impl->_topic, wqos, listener);
+    if (_impl->_datawriter == nullptr) {
+        std::cerr << "Failed to create DataWriter" << std::endl;
+        return false;
+    }
+    _frame_id = _name;
+    return true;
+  }
+
+  bool CarlaDepthCameraPublisher::InitInfo() {
+    if (_impl_info->_type == nullptr) {
+        std::cerr << "Invalid TypeSupport" << std::endl;
+        return false;
+    }
+
+    efd::DomainParticipantQos pqos = efd::PARTICIPANT_QOS_DEFAULT;
+    pqos.name(_name);
+    auto factory = efd::DomainParticipantFactory::get_instance();
+    _impl_info->_participant = factory->create_participant(0, pqos);
+    if (_impl_info->_participant == nullptr) {
+        std::cerr << "Failed to create DomainParticipant" << std::endl;
+        return false;
+    }
+    _impl_info->_type.register_type(_impl_info->_participant);
+
+    efd::PublisherQos pubqos = efd::PUBLISHER_QOS_DEFAULT;
+    _impl_info->_publisher = _impl_info->_participant->create_publisher(pubqos, nullptr);
+    if (_impl_info->_publisher == nullptr) {
+      std::cerr << "Failed to create Publisher" << std::endl;
+      return false;
+    }
+
+    efd::TopicQos tqos = efd::TOPIC_QOS_DEFAULT;
+    const std::string publisher_type {"/camera_info"};
+    const std::string base { "rt/carla/" };
+    std::string topic_name = base;
+    if (!_parent.empty())
+      topic_name += _parent + "/";
+    topic_name += _name;
+    topic_name += publisher_type;
+    _impl_info->_topic = _impl_info->_participant->create_topic(topic_name, _impl_info->_type->getName(), tqos);
+    if (_impl_info->_topic == nullptr) {
+        std::cerr << "Failed to create Topic" << std::endl;
+        return false;
+    }
+    efd::DataWriterQos wqos = efd::DATAWRITER_QOS_DEFAULT;
+    efd::DataWriterListener* listener = (efd::DataWriterListener*)_impl_info->_listener._impl.get();
+    _impl_info->_datawriter = _impl_info->_publisher->create_datawriter(_impl_info->_topic, wqos, listener);
+    if (_impl_info->_datawriter == nullptr) {
+        std::cerr << "Failed to create DataWriter" << std::endl;
+        return false;
+    }
+
+    _frame_id = _name;
+    return true;
+  }
+
+  bool CarlaDepthCameraPublisher::Publish() {
+    return PublishImage() && PublishInfo();
+  }
+
+  bool CarlaDepthCameraPublisher::PublishImage() {
+    eprosima::fastrtps::rtps::InstanceHandle_t instance_handle;
+    eprosima::fastrtps::types::ReturnCode_t rcode = _impl->_datawriter->write(&_impl->_image, instance_handle);
+    if (rcode == erc::ReturnCodeValue::RETCODE_OK) {
+        return true;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_ERROR) {
+        std::cerr << "RETCODE_ERROR" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_UNSUPPORTED) {
+        std::cerr << "RETCODE_UNSUPPORTED" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_BAD_PARAMETER) {
+        std::cerr << "RETCODE_BAD_PARAMETER" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_PRECONDITION_NOT_MET) {
+        std::cerr << "RETCODE_PRECONDITION_NOT_MET" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_OUT_OF_RESOURCES) {
+        std::cerr << "RETCODE_OUT_OF_RESOURCES" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_NOT_ENABLED) {
+        std::cerr << "RETCODE_NOT_ENABLED" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_IMMUTABLE_POLICY) {
+        std::cerr << "RETCODE_IMMUTABLE_POLICY" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_INCONSISTENT_POLICY) {
+        std::cerr << "RETCODE_INCONSISTENT_POLICY" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_ALREADY_DELETED) {
+        std::cerr << "RETCODE_ALREADY_DELETED" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_TIMEOUT) {
+        std::cerr << "RETCODE_TIMEOUT" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_NO_DATA) {
+        std::cerr << "RETCODE_NO_DATA" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_ILLEGAL_OPERATION) {
+        std::cerr << "RETCODE_ILLEGAL_OPERATION" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_NOT_ALLOWED_BY_SECURITY) {
+        std::cerr << "RETCODE_NOT_ALLOWED_BY_SECURITY" << std::endl;
+        return false;
+    }
+    std::cerr << "UNKNOWN" << std::endl;
+    return false;
+  }
+
+  bool CarlaDepthCameraPublisher::PublishInfo() {
+    eprosima::fastrtps::rtps::InstanceHandle_t instance_handle;
+    eprosima::fastrtps::types::ReturnCode_t rcode = _impl_info->_datawriter->write(&_impl_info->_info, instance_handle);
+    if (rcode == erc::ReturnCodeValue::RETCODE_OK) {
+        return true;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_ERROR) {
+        std::cerr << "RETCODE_ERROR" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_UNSUPPORTED) {
+        std::cerr << "RETCODE_UNSUPPORTED" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_BAD_PARAMETER) {
+        std::cerr << "RETCODE_BAD_PARAMETER" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_PRECONDITION_NOT_MET) {
+        std::cerr << "RETCODE_PRECONDITION_NOT_MET" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_OUT_OF_RESOURCES) {
+        std::cerr << "RETCODE_OUT_OF_RESOURCES" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_NOT_ENABLED) {
+        std::cerr << "RETCODE_NOT_ENABLED" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_IMMUTABLE_POLICY) {
+        std::cerr << "RETCODE_IMMUTABLE_POLICY" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_INCONSISTENT_POLICY) {
+        std::cerr << "RETCODE_INCONSISTENT_POLICY" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_ALREADY_DELETED) {
+        std::cerr << "RETCODE_ALREADY_DELETED" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_TIMEOUT) {
+        std::cerr << "RETCODE_TIMEOUT" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_NO_DATA) {
+        std::cerr << "RETCODE_NO_DATA" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_ILLEGAL_OPERATION) {
+        std::cerr << "RETCODE_ILLEGAL_OPERATION" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_NOT_ALLOWED_BY_SECURITY) {
+        std::cerr << "RETCODE_NOT_ALLOWED_BY_SECURITY" << std::endl;
+        return false;
+    }
+    std::cerr << "UNKNOWN" << std::endl;
+    return false;
+  }
+
+  void CarlaDepthCameraPublisher::SetImageData(int32_t seconds, uint32_t nanoseconds, size_t height, size_t width, const uint8_t* data) {    std::vector<uint8_t> vector_data;
+    const size_t size = height * width * 4;
+    vector_data.resize(size);
+    std::memcpy(&vector_data[0], &data[0], size);
+    SetData(seconds, nanoseconds,height, width, std::move(vector_data));
+  }
+
+  void CarlaDepthCameraPublisher::SetInfoRegionOfInterest( uint32_t x_offset, uint32_t y_offset, uint32_t height, uint32_t width, bool do_rectify) {
+    sensor_msgs::msg::RegionOfInterest roi;
+    roi.x_offset(x_offset);
+    roi.y_offset(y_offset);
+    roi.height(height);
+    roi.width(width);
+    roi.do_rectify(do_rectify);
+    _impl_info->_info.roi(roi);
+  }
+
+  void CarlaDepthCameraPublisher::SetData(int32_t seconds, uint32_t nanoseconds, size_t height, size_t width, std::vector<uint8_t>&& data) {
+    builtin_interfaces::msg::Time time;
+    time.sec(seconds);
+    time.nanosec(nanoseconds);
+
+    std_msgs::msg::Header header;
+    header.stamp(std::move(time));
+    header.frame_id(_frame_id);
+
+    _impl->_image.header(std::move(header));
+    _impl->_image.width(width);
+    _impl->_image.height(height);
+    _impl->_image.encoding("bgra8");
+    _impl->_image.is_bigendian(0);
+    _impl->_image.step(_impl->_image.width() * sizeof(uint8_t) * 4);
+    _impl->_image.data(std::move(data)); //https://github.com/eProsima/Fast-DDS/issues/2330
+  }
+
+  void CarlaDepthCameraPublisher::SetCameraInfoData(int32_t seconds, uint32_t nanoseconds) {
+    builtin_interfaces::msg::Time time;
+    time.sec(seconds);
+    time.nanosec(nanoseconds);
+
+    std_msgs::msg::Header header;
+    header.stamp(std::move(time));
+    header.frame_id(_frame_id);
+    _impl_info->_info.header(header);
+  }
+
+  CarlaDepthCameraPublisher::CarlaDepthCameraPublisher(const char* ros_name, const char* parent) :
+  _impl(std::make_shared<CarlaDepthCameraPublisherImpl>()),
+  _impl_info(std::make_shared<CarlaCameraInfoPublisherImpl>()) {
+    _name = ros_name;
+    _parent = parent;
+  }
+
+  CarlaDepthCameraPublisher::~CarlaDepthCameraPublisher() {
+      if (!_impl)
+          return;
+
+      if (_impl->_datawriter)
+          _impl->_publisher->delete_datawriter(_impl->_datawriter);
+
+      if (_impl->_publisher)
+          _impl->_participant->delete_publisher(_impl->_publisher);
+
+      if (_impl->_topic)
+          _impl->_participant->delete_topic(_impl->_topic);
+
+      if (_impl->_participant)
+          efd::DomainParticipantFactory::get_instance()->delete_participant(_impl->_participant);
+
+      if (!_impl_info)
+        return;
+
+      if (_impl_info->_datawriter)
+          _impl_info->_publisher->delete_datawriter(_impl_info->_datawriter);
+
+      if (_impl_info->_publisher)
+          _impl_info->_participant->delete_publisher(_impl_info->_publisher);
+
+      if (_impl_info->_topic)
+          _impl_info->_participant->delete_topic(_impl_info->_topic);
+
+      if (_impl_info->_participant)
+          efd::DomainParticipantFactory::get_instance()->delete_participant(_impl_info->_participant);
+  }
+
+  CarlaDepthCameraPublisher::CarlaDepthCameraPublisher(const CarlaDepthCameraPublisher& other) {
+    _frame_id = other._frame_id;
+    _name = other._name;
+    _parent = other._parent;
+    _impl = other._impl;
+    _impl_info = other._impl_info;
+  }
+
+  CarlaDepthCameraPublisher& CarlaDepthCameraPublisher::operator=(const CarlaDepthCameraPublisher& other) {
+    _frame_id = other._frame_id;
+    _name = other._name;
+    _parent = other._parent;
+    _impl = other._impl;
+    _impl_info = other._impl_info;
+
+    return *this;
+  }
+
+  CarlaDepthCameraPublisher::CarlaDepthCameraPublisher(CarlaDepthCameraPublisher&& other) {
+    _frame_id = std::move(other._frame_id);
+    _name = std::move(other._name);
+    _parent = std::move(other._parent);
+    _impl = std::move(other._impl);
+    _impl_info = std::move(other._impl_info);
+
+  }
+
+  CarlaDepthCameraPublisher& CarlaDepthCameraPublisher::operator=(CarlaDepthCameraPublisher&& other) {
+    _frame_id = std::move(other._frame_id);
+    _name = std::move(other._name);
+    _parent = std::move(other._parent);
+    _impl = std::move(other._impl);
+    _impl_info = std::move(other._impl_info);
+
+    return *this;
+  }
+}
+}

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

@@ -0,0 +1,51 @@
+// Copyright (c) 2022 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 <https://opensource.org/licenses/MIT>.
+
+#pragma once
+#define _GLIBCXX_USE_CXX11_ABI 0
+
+#include <memory>
+#include <vector>
+
+#include "CarlaPublisher.h"
+
+namespace carla {
+namespace ros2 {
+
+  struct CarlaDepthCameraPublisherImpl;
+  struct CarlaCameraInfoPublisherImpl;
+
+  class CarlaDepthCameraPublisher : public CarlaPublisher {
+    public:
+      CarlaDepthCameraPublisher(const char* ros_name = "", const char* parent = "");
+      ~CarlaDepthCameraPublisher();
+      CarlaDepthCameraPublisher(const CarlaDepthCameraPublisher&);
+      CarlaDepthCameraPublisher& operator=(const CarlaDepthCameraPublisher&);
+      CarlaDepthCameraPublisher(CarlaDepthCameraPublisher&&);
+      CarlaDepthCameraPublisher& operator=(CarlaDepthCameraPublisher&&);
+
+      bool Init();
+      void InitInfoData(uint32_t x_offset, uint32_t y_offset, uint32_t height, uint32_t width, float fov, bool do_rectify);
+      bool Publish();
+
+      bool HasBeenInitialized() const;
+      void SetImageData(int32_t seconds, uint32_t nanoseconds, size_t height, size_t width, const uint8_t* data);
+      void SetCameraInfoData(int32_t seconds, uint32_t nanoseconds);
+      const char* type() const override { return "depth camera"; }
+
+    private:
+      bool InitImage();
+      bool InitInfo();
+      bool PublishImage();
+      bool PublishInfo();
+
+      void SetInfoRegionOfInterest( uint32_t x_offset, uint32_t y_offset, uint32_t height, uint32_t width, bool do_rectify);
+      void SetData(int32_t seconds, uint32_t nanoseconds, size_t height, size_t width, std::vector<uint8_t>&& data);
+
+    private:
+      std::shared_ptr<CarlaDepthCameraPublisherImpl> _impl;
+      std::shared_ptr<CarlaCameraInfoPublisherImpl> _impl_info;
+  };
+}
+}

LibCarla/source/carla/ros2/publishers/CarlaGNSSPublisher.cpp

@@ -0,0 +1,221 @@
+#define _GLIBCXX_USE_CXX11_ABI 0
+
+#include "CarlaGNSSPublisher.h"
+
+#include <string>
+
+#include "carla/ros2/types/NavSatFixPubSubTypes.h"
+#include "carla/ros2/listeners/CarlaListener.h"
+
+#include <fastdds/dds/domain/DomainParticipant.hpp>
+#include <fastdds/dds/publisher/Publisher.hpp>
+#include <fastdds/dds/topic/Topic.hpp>
+#include <fastdds/dds/publisher/DataWriter.hpp>
+#include <fastdds/dds/topic/TypeSupport.hpp>
+
+#include <fastdds/dds/domain/qos/DomainParticipantQos.hpp>
+#include <fastdds/dds/domain/DomainParticipantFactory.hpp>
+#include <fastdds/dds/publisher/qos/PublisherQos.hpp>
+#include <fastdds/dds/topic/qos/TopicQos.hpp>
+
+#include <fastrtps/attributes/ParticipantAttributes.h>
+#include <fastrtps/qos/QosPolicies.h>
+#include <fastdds/dds/publisher/qos/DataWriterQos.hpp>
+#include <fastdds/dds/publisher/DataWriterListener.hpp>
+
+
+namespace carla {
+namespace ros2 {
+  namespace efd = eprosima::fastdds::dds;
+  using erc = eprosima::fastrtps::types::ReturnCode_t;
+
+  struct CarlaGNSSPublisherImpl {
+    efd::DomainParticipant* _participant { nullptr };
+    efd::Publisher* _publisher { nullptr };
+    efd::Topic* _topic { nullptr };
+    efd::DataWriter* _datawriter { nullptr };
+    efd::TypeSupport _type { new sensor_msgs::msg::NavSatFixPubSubType() };
+    CarlaListener _listener {};
+    sensor_msgs::msg::NavSatFix _nav {};
+  };
+
+  bool CarlaGNSSPublisher::Init() {
+    if (_impl->_type == nullptr) {
+        std::cerr << "Invalid TypeSupport" << std::endl;
+        return false;
+    }
+
+    efd::DomainParticipantQos pqos = efd::PARTICIPANT_QOS_DEFAULT;
+    pqos.name(_name);
+    auto factory = efd::DomainParticipantFactory::get_instance();
+    _impl->_participant = factory->create_participant(0, pqos);
+    if (_impl->_participant == nullptr) {
+        std::cerr << "Failed to create DomainParticipant" << std::endl;
+        return false;
+    }
+    _impl->_type.register_type(_impl->_participant);
+
+    efd::PublisherQos pubqos = efd::PUBLISHER_QOS_DEFAULT;
+    _impl->_publisher = _impl->_participant->create_publisher(pubqos, nullptr);
+    if (_impl->_publisher == nullptr) {
+      std::cerr << "Failed to create Publisher" << std::endl;
+      return false;
+    }
+
+    efd::TopicQos tqos = efd::TOPIC_QOS_DEFAULT;
+    const std::string base { "rt/carla/" };
+    std::string topic_name = base;
+    if (!_parent.empty())
+      topic_name += _parent + "/";
+    topic_name += _name;
+    _impl->_topic = _impl->_participant->create_topic(topic_name, _impl->_type->getName(), tqos);
+    if (_impl->_topic == nullptr) {
+        std::cerr << "Failed to create Topic" << std::endl;
+        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 = (efd::DataWriterListener*)_impl->_listener._impl.get();
+    _impl->_datawriter = _impl->_publisher->create_datawriter(_impl->_topic, wqos, listener);
+    if (_impl->_datawriter == nullptr) {
+        std::cerr << "Failed to create DataWriter" << std::endl;
+        return false;
+    }
+    _frame_id = _name;
+    return true;
+  }
+
+  bool CarlaGNSSPublisher::Publish() {
+    eprosima::fastrtps::rtps::InstanceHandle_t instance_handle;
+    eprosima::fastrtps::types::ReturnCode_t rcode = _impl->_datawriter->write(&_impl->_nav, instance_handle);
+    if (rcode == erc::ReturnCodeValue::RETCODE_OK) {
+        return true;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_ERROR) {
+        std::cerr << "RETCODE_ERROR" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_UNSUPPORTED) {
+        std::cerr << "RETCODE_UNSUPPORTED" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_BAD_PARAMETER) {
+        std::cerr << "RETCODE_BAD_PARAMETER" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_PRECONDITION_NOT_MET) {
+        std::cerr << "RETCODE_PRECONDITION_NOT_MET" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_OUT_OF_RESOURCES) {
+        std::cerr << "RETCODE_OUT_OF_RESOURCES" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_NOT_ENABLED) {
+        std::cerr << "RETCODE_NOT_ENABLED" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_IMMUTABLE_POLICY) {
+        std::cerr << "RETCODE_IMMUTABLE_POLICY" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_INCONSISTENT_POLICY) {
+        std::cerr << "RETCODE_INCONSISTENT_POLICY" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_ALREADY_DELETED) {
+        std::cerr << "RETCODE_ALREADY_DELETED" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_TIMEOUT) {
+        std::cerr << "RETCODE_TIMEOUT" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_NO_DATA) {
+        std::cerr << "RETCODE_NO_DATA" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_ILLEGAL_OPERATION) {
+        std::cerr << "RETCODE_ILLEGAL_OPERATION" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_NOT_ALLOWED_BY_SECURITY) {
+        std::cerr << "RETCODE_NOT_ALLOWED_BY_SECURITY" << std::endl;
+        return false;
+    }
+    std::cerr << "UNKNOWN return code: " << rcode() << std::endl;
+    return false;
+  }
+
+  void CarlaGNSSPublisher::SetData(int32_t seconds, uint32_t nanoseconds, const double* data) {
+    builtin_interfaces::msg::Time time;
+    time.sec(seconds);
+    time.nanosec(nanoseconds);
+
+    std_msgs::msg::Header header;
+    header.stamp(std::move(time));
+    header.frame_id(_frame_id);
+
+    _impl->_nav.header(std::move(header));
+    _impl->_nav.latitude(*data++);
+    _impl->_nav.longitude(*data++);
+    _impl->_nav.altitude(*data++);
+  }
+
+  CarlaGNSSPublisher::CarlaGNSSPublisher(const char* ros_name, const char* parent) :
+  _impl(std::make_shared<CarlaGNSSPublisherImpl>()) {
+    _name = ros_name;
+    _parent = parent;
+  }
+
+  CarlaGNSSPublisher::~CarlaGNSSPublisher() {
+      if (!_impl)
+          return;
+
+      if (_impl->_datawriter)
+          _impl->_publisher->delete_datawriter(_impl->_datawriter);
+
+      if (_impl->_publisher)
+          _impl->_participant->delete_publisher(_impl->_publisher);
+
+      if (_impl->_topic)
+          _impl->_participant->delete_topic(_impl->_topic);
+
+      if (_impl->_participant)
+          efd::DomainParticipantFactory::get_instance()->delete_participant(_impl->_participant);
+  }
+
+  CarlaGNSSPublisher::CarlaGNSSPublisher(const CarlaGNSSPublisher& other) {
+    _frame_id = other._frame_id;
+    _name = other._name;
+    _parent = other._parent;
+    _impl = other._impl;
+  }
+
+  CarlaGNSSPublisher& CarlaGNSSPublisher::operator=(const CarlaGNSSPublisher& other) {
+    _frame_id = other._frame_id;
+    _name = other._name;
+    _parent = other._parent;
+    _impl = other._impl;
+
+    return *this;
+  }
+
+  CarlaGNSSPublisher::CarlaGNSSPublisher(CarlaGNSSPublisher&& other) {
+    _frame_id = std::move(other._frame_id);
+    _name = std::move(other._name);
+    _parent = std::move(other._parent);
+    _impl = std::move(other._impl);
+  }
+
+  CarlaGNSSPublisher& CarlaGNSSPublisher::operator=(CarlaGNSSPublisher&& other) {
+    _frame_id = std::move(other._frame_id);
+    _name = std::move(other._name);
+    _parent = std::move(other._parent);
+    _impl = std::move(other._impl);
+
+    return *this;
+  }
+}
+}

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

@@ -0,0 +1,35 @@
+// Copyright (c) 2022 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 <https://opensource.org/licenses/MIT>.
+
+#pragma once
+#define _GLIBCXX_USE_CXX11_ABI 0
+
+#include <memory>
+
+#include "CarlaPublisher.h"
+
+namespace carla {
+namespace ros2 {
+
+  struct CarlaGNSSPublisherImpl;
+
+  class CarlaGNSSPublisher : public CarlaPublisher {
+    public:
+      CarlaGNSSPublisher(const char* ros_name = "", const char* parent = "");
+      ~CarlaGNSSPublisher();
+      CarlaGNSSPublisher(const CarlaGNSSPublisher&);
+      CarlaGNSSPublisher& operator=(const CarlaGNSSPublisher&);
+      CarlaGNSSPublisher(CarlaGNSSPublisher&&);
+      CarlaGNSSPublisher& operator=(CarlaGNSSPublisher&&);
+
+      bool Init();
+      bool Publish();
+      void SetData(int32_t seconds, uint32_t nanoseconds, const double* data);
+      const char* type() const override { return "gnss"; }
+
+    private:
+      std::shared_ptr<CarlaGNSSPublisherImpl> _impl;
+  };
+}
+}

LibCarla/source/carla/ros2/publishers/CarlaIMUPublisher.cpp

@@ -0,0 +1,254 @@
+#define _GLIBCXX_USE_CXX11_ABI 0
+
+#include "CarlaIMUPublisher.h"
+
+#include <string>
+
+#include "carla/ros2/types/ImuPubSubTypes.h"
+#include "carla/ros2/listeners/CarlaListener.h"
+
+#include <fastdds/dds/domain/DomainParticipant.hpp>
+#include <fastdds/dds/publisher/Publisher.hpp>
+#include <fastdds/dds/topic/Topic.hpp>
+#include <fastdds/dds/publisher/DataWriter.hpp>
+#include <fastdds/dds/topic/TypeSupport.hpp>
+
+#include <fastdds/dds/domain/qos/DomainParticipantQos.hpp>
+#include <fastdds/dds/domain/DomainParticipantFactory.hpp>
+#include <fastdds/dds/publisher/qos/PublisherQos.hpp>
+#include <fastdds/dds/topic/qos/TopicQos.hpp>
+
+#include <fastrtps/attributes/ParticipantAttributes.h>
+#include <fastrtps/qos/QosPolicies.h>
+#include <fastdds/dds/publisher/qos/DataWriterQos.hpp>
+#include <fastdds/dds/publisher/DataWriterListener.hpp>
+
+
+namespace carla {
+namespace ros2 {
+  namespace efd = eprosima::fastdds::dds;
+  using erc = eprosima::fastrtps::types::ReturnCode_t;
+
+  struct CarlaIMUPublisherImpl {
+    efd::DomainParticipant* _participant { nullptr };
+    efd::Publisher* _publisher { nullptr };
+    efd::Topic* _topic { nullptr };
+    efd::DataWriter* _datawriter { nullptr };
+    efd::TypeSupport _type { new sensor_msgs::msg::ImuPubSubType() };
+    CarlaListener _listener {};
+    sensor_msgs::msg::Imu _imu {};
+  };
+
+  bool CarlaIMUPublisher::Init() {
+    if (_impl->_type == nullptr) {
+        std::cerr << "Invalid TypeSupport" << std::endl;
+        return false;
+    }
+
+    efd::DomainParticipantQos pqos = efd::PARTICIPANT_QOS_DEFAULT;
+    pqos.name(_name);
+    auto factory = efd::DomainParticipantFactory::get_instance();
+    _impl->_participant = factory->create_participant(0, pqos);
+    if (_impl->_participant == nullptr) {
+        std::cerr << "Failed to create DomainParticipant" << std::endl;
+        return false;
+    }
+    _impl->_type.register_type(_impl->_participant);
+
+    efd::PublisherQos pubqos = efd::PUBLISHER_QOS_DEFAULT;
+    _impl->_publisher = _impl->_participant->create_publisher(pubqos, nullptr);
+    if (_impl->_publisher == nullptr) {
+      std::cerr << "Failed to create Publisher" << std::endl;
+      return false;
+    }
+
+    efd::TopicQos tqos = efd::TOPIC_QOS_DEFAULT;
+    const std::string base { "rt/carla/" };
+    std::string topic_name = base;
+    if (!_parent.empty())
+      topic_name += _parent + "/";
+    topic_name += _name;
+    _impl->_topic = _impl->_participant->create_topic(topic_name, _impl->_type->getName(), tqos);
+    if (_impl->_topic == nullptr) {
+        std::cerr << "Failed to create Topic" << std::endl;
+        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 = (efd::DataWriterListener*)_impl->_listener._impl.get();
+    _impl->_datawriter = _impl->_publisher->create_datawriter(_impl->_topic, wqos, listener);
+    if (_impl->_datawriter == nullptr) {
+        std::cerr << "Failed to create DataWriter" << std::endl;
+        return false;
+    }
+    _frame_id = _name;
+    return true;
+  }
+
+  bool CarlaIMUPublisher::Publish() {
+    eprosima::fastrtps::rtps::InstanceHandle_t instance_handle;
+    eprosima::fastrtps::types::ReturnCode_t rcode = _impl->_datawriter->write(&_impl->_imu, instance_handle);
+    if (rcode == erc::ReturnCodeValue::RETCODE_OK) {
+        return true;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_ERROR) {
+        std::cerr << "RETCODE_ERROR" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_UNSUPPORTED) {
+        std::cerr << "RETCODE_UNSUPPORTED" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_BAD_PARAMETER) {
+        std::cerr << "RETCODE_BAD_PARAMETER" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_PRECONDITION_NOT_MET) {
+        std::cerr << "RETCODE_PRECONDITION_NOT_MET" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_OUT_OF_RESOURCES) {
+        std::cerr << "RETCODE_OUT_OF_RESOURCES" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_NOT_ENABLED) {
+        std::cerr << "RETCODE_NOT_ENABLED" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_IMMUTABLE_POLICY) {
+        std::cerr << "RETCODE_IMMUTABLE_POLICY" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_INCONSISTENT_POLICY) {
+        std::cerr << "RETCODE_INCONSISTENT_POLICY" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_ALREADY_DELETED) {
+        std::cerr << "RETCODE_ALREADY_DELETED" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_TIMEOUT) {
+        std::cerr << "RETCODE_TIMEOUT" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_NO_DATA) {
+        std::cerr << "RETCODE_NO_DATA" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_ILLEGAL_OPERATION) {
+        std::cerr << "RETCODE_ILLEGAL_OPERATION" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_NOT_ALLOWED_BY_SECURITY) {
+        std::cerr << "RETCODE_NOT_ALLOWED_BY_SECURITY" << std::endl;
+        return false;
+    }
+    std::cerr << "UNKNOWN" << std::endl;
+    return false;
+  }
+
+  void CarlaIMUPublisher::SetData(int32_t seconds, uint32_t nanoseconds, float* pAccelerometer, float* pGyroscope, float compass) {
+    geometry_msgs::msg::Vector3 gyroscope;
+    geometry_msgs::msg::Vector3 linear_acceleration;
+    const float ax = *pAccelerometer++;
+    const float ay = *pAccelerometer++;
+    const float az = *pAccelerometer++;
+    linear_acceleration.x(ax);
+    linear_acceleration.y(ay);
+    linear_acceleration.z(az);
+    const float gx = *pGyroscope++;
+    const float gy = *pGyroscope++;
+    const float gz = *pGyroscope++;
+    gyroscope.x(gx);
+    gyroscope.y(gy);
+    gyroscope.z(gz);
+
+    builtin_interfaces::msg::Time time;
+    time.sec(seconds);
+    time.nanosec(nanoseconds);
+
+    std_msgs::msg::Header header;
+    header.stamp(std::move(time));
+    header.frame_id(_frame_id);
+
+    geometry_msgs::msg::Quaternion orientation;
+
+    const float rx = 0.0f;                           // pitch
+    const float ry = (M_PIf32 / 2.0f) - compass;     // yaw
+    const float rz = 0.0f;                           // roll
+
+    const float cr = cosf(rz * 0.5f);
+    const float sr = sinf(rz * 0.5f);
+    const float cp = cosf(rx * 0.5f);
+    const float sp = sinf(rx * 0.5f);
+    const float cy = cosf(ry * 0.5f);
+    const float sy = sinf(ry * 0.5f);
+
+    orientation.w(cr * cp * cy + sr * sp * sy);
+    orientation.x(sr * cp * cy - cr * sp * sy);
+    orientation.y(cr * sp * cy + sr * cp * sy);
+    orientation.z(cr * cp * sy - sr * sp * cy);
+
+    _impl->_imu.header(std::move(header));
+    _impl->_imu.orientation(orientation);
+    _impl->_imu.angular_velocity(gyroscope);
+    _impl->_imu.linear_acceleration(linear_acceleration);
+  }
+
+  CarlaIMUPublisher::CarlaIMUPublisher(const char* ros_name, const char* parent) :
+  _impl(std::make_shared<CarlaIMUPublisherImpl>()) {
+    _name = ros_name;
+    _parent = parent;
+  }
+
+  CarlaIMUPublisher::~CarlaIMUPublisher() {
+      if (!_impl)
+          return;
+
+      if (_impl->_datawriter)
+          _impl->_publisher->delete_datawriter(_impl->_datawriter);
+
+      if (_impl->_publisher)
+          _impl->_participant->delete_publisher(_impl->_publisher);
+
+      if (_impl->_topic)
+          _impl->_participant->delete_topic(_impl->_topic);
+
+      if (_impl->_participant)
+          efd::DomainParticipantFactory::get_instance()->delete_participant(_impl->_participant);
+  }
+
+  CarlaIMUPublisher::CarlaIMUPublisher(const CarlaIMUPublisher& other) {
+    _frame_id = other._frame_id;
+    _name = other._name;
+    _parent = other._parent;
+    _impl = other._impl;
+  }
+
+  CarlaIMUPublisher& CarlaIMUPublisher::operator=(const CarlaIMUPublisher& other) {
+    _frame_id = other._frame_id;
+    _name = other._name;
+    _parent = other._parent;
+    _impl = other._impl;
+
+    return *this;
+  }
+
+  CarlaIMUPublisher::CarlaIMUPublisher(CarlaIMUPublisher&& other) {
+    _frame_id = std::move(other._frame_id);
+    _name = std::move(other._name);
+    _parent = std::move(other._parent);
+    _impl = std::move(other._impl);
+  }
+
+  CarlaIMUPublisher& CarlaIMUPublisher::operator=(CarlaIMUPublisher&& other) {
+    _frame_id = std::move(other._frame_id);
+    _name = std::move(other._name);
+    _parent = std::move(other._parent);
+    _impl = std::move(other._impl);
+
+    return *this;
+  }
+}
+}

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

@@ -0,0 +1,35 @@
+// Copyright (c) 2022 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 <https://opensource.org/licenses/MIT>.
+
+#pragma once
+#define _GLIBCXX_USE_CXX11_ABI 0
+
+#include <memory>
+
+#include "CarlaPublisher.h"
+
+namespace carla {
+namespace ros2 {
+
+  struct CarlaIMUPublisherImpl;
+
+  class CarlaIMUPublisher : public CarlaPublisher {
+    public:
+      CarlaIMUPublisher(const char* ros_name = "", const char* parent = "");
+      ~CarlaIMUPublisher();
+      CarlaIMUPublisher(const CarlaIMUPublisher&);
+      CarlaIMUPublisher& operator=(const CarlaIMUPublisher&);
+      CarlaIMUPublisher(CarlaIMUPublisher&&);
+      CarlaIMUPublisher& operator=(CarlaIMUPublisher&&);
+
+      bool Init();
+      bool Publish();
+      void SetData(int32_t seconds, uint32_t nanoseconds, float* accelerometer, float* gyroscope, float compass);
+      const char* type() const override { return "inertial measurement unit"; }
+
+    private:
+      std::shared_ptr<CarlaIMUPublisherImpl> _impl;
+  };
+}
+}

LibCarla/source/carla/ros2/publishers/CarlaISCameraPublisher.cpp

@@ -0,0 +1,414 @@
+#define _GLIBCXX_USE_CXX11_ABI 0
+
+#include "CarlaISCameraPublisher.h"
+
+#include <string>
+
+#include "carla/ros2/types/ImagePubSubTypes.h"
+#include "carla/ros2/types/CameraInfoPubSubTypes.h"
+#include "carla/ros2/listeners/CarlaListener.h"
+
+#include <fastdds/dds/domain/DomainParticipant.hpp>
+#include <fastdds/dds/publisher/Publisher.hpp>
+#include <fastdds/dds/topic/Topic.hpp>
+#include <fastdds/dds/publisher/DataWriter.hpp>
+#include <fastdds/dds/topic/TypeSupport.hpp>
+
+#include <fastdds/dds/domain/qos/DomainParticipantQos.hpp>
+#include <fastdds/dds/domain/DomainParticipantFactory.hpp>
+#include <fastdds/dds/publisher/qos/PublisherQos.hpp>
+#include <fastdds/dds/topic/qos/TopicQos.hpp>
+
+#include <fastrtps/attributes/ParticipantAttributes.h>
+#include <fastrtps/qos/QosPolicies.h>
+#include <fastdds/dds/publisher/qos/DataWriterQos.hpp>
+#include <fastdds/dds/publisher/DataWriterListener.hpp>
+
+namespace carla {
+namespace ros2 {
+
+  namespace efd = eprosima::fastdds::dds;
+  using erc = eprosima::fastrtps::types::ReturnCode_t;
+
+  struct CarlaISCameraPublisherImpl {
+    efd::DomainParticipant* _participant { nullptr };
+    efd::Publisher* _publisher { nullptr };
+    efd::Topic* _topic { nullptr };
+    efd::DataWriter* _datawriter { nullptr };
+    efd::TypeSupport _type { new sensor_msgs::msg::ImagePubSubType() };
+    CarlaListener _listener {};
+    sensor_msgs::msg::Image _image {};
+  };
+
+  struct CarlaCameraInfoPublisherImpl {
+    efd::DomainParticipant* _participant { nullptr };
+    efd::Publisher* _publisher { nullptr };
+    efd::Topic* _topic { nullptr };
+    efd::DataWriter* _datawriter { nullptr };
+    efd::TypeSupport _type { new sensor_msgs::msg::CameraInfoPubSubType() };
+    CarlaListener _listener {};
+    bool _init {false};
+    sensor_msgs::msg::CameraInfo _info {};
+  };
+
+  bool CarlaISCameraPublisher::HasBeenInitialized() const {
+    return _impl_info->_init;
+  }
+
+  void CarlaISCameraPublisher::InitInfoData(uint32_t x_offset, uint32_t y_offset, uint32_t height, uint32_t width, float fov, bool do_rectify) {
+    _impl_info->_info = std::move(sensor_msgs::msg::CameraInfo(height, width, fov));
+    SetInfoRegionOfInterest(x_offset, y_offset, height, width, do_rectify);
+    _impl_info->_init = true;
+  }
+
+  bool CarlaISCameraPublisher::Init() {
+    return InitImage() && InitInfo();
+  }
+
+  bool CarlaISCameraPublisher::InitImage() {
+    if (_impl->_type == nullptr) {
+        std::cerr << "Invalid TypeSupport" << std::endl;
+        return false;
+    }
+    efd::DomainParticipantQos pqos = efd::PARTICIPANT_QOS_DEFAULT;
+    pqos.name(_name);
+    auto factory = efd::DomainParticipantFactory::get_instance();
+    _impl->_participant = factory->create_participant(0, pqos);
+    if (_impl->_participant == nullptr) {
+        std::cerr << "Failed to create DomainParticipant" << std::endl;
+        return false;
+    }
+    _impl->_type.register_type(_impl->_participant);
+
+    efd::PublisherQos pubqos = efd::PUBLISHER_QOS_DEFAULT;
+    _impl->_publisher = _impl->_participant->create_publisher(pubqos, nullptr);
+    if (_impl->_publisher == nullptr) {
+      std::cerr << "Failed to create Publisher" << std::endl;
+      return false;
+    }
+
+    efd::TopicQos tqos = efd::TOPIC_QOS_DEFAULT;
+    const std::string publisher_type {"/image"};
+    const std::string base { "rt/carla/" };
+    std::string topic_name = base;
+    if (!_parent.empty())
+      topic_name += _parent + "/";
+    topic_name += _name;
+    topic_name += publisher_type;
+    _impl->_topic = _impl->_participant->create_topic(topic_name, _impl->_type->getName(), tqos);
+    if (_impl->_topic == nullptr) {
+        std::cerr << "Failed to create Topic" << std::endl;
+        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 = (efd::DataWriterListener*)_impl->_listener._impl.get();
+    _impl->_datawriter = _impl->_publisher->create_datawriter(_impl->_topic, wqos, listener);
+    if (_impl->_datawriter == nullptr) {
+        std::cerr << "Failed to create DataWriter" << std::endl;
+        return false;
+    }
+    _frame_id = _name;
+    return true;
+  }
+
+  bool CarlaISCameraPublisher::InitInfo() {
+    if (_impl_info->_type == nullptr) {
+        std::cerr << "Invalid TypeSupport" << std::endl;
+        return false;
+    }
+
+    efd::DomainParticipantQos pqos = efd::PARTICIPANT_QOS_DEFAULT;
+    pqos.name(_name);
+    auto factory = efd::DomainParticipantFactory::get_instance();
+    _impl_info->_participant = factory->create_participant(0, pqos);
+    if (_impl_info->_participant == nullptr) {
+        std::cerr << "Failed to create DomainParticipant" << std::endl;
+        return false;
+    }
+    _impl_info->_type.register_type(_impl_info->_participant);
+
+    efd::PublisherQos pubqos = efd::PUBLISHER_QOS_DEFAULT;
+    _impl_info->_publisher = _impl_info->_participant->create_publisher(pubqos, nullptr);
+    if (_impl_info->_publisher == nullptr) {
+      std::cerr << "Failed to create Publisher" << std::endl;
+      return false;
+    }
+
+    efd::TopicQos tqos = efd::TOPIC_QOS_DEFAULT;
+    const std::string publisher_type {"/camera_info"};
+    const std::string base { "rt/carla/" };
+    std::string topic_name = base;
+    if (!_parent.empty())
+      topic_name += _parent + "/";
+    topic_name += _name;
+    topic_name += publisher_type;
+    _impl_info->_topic = _impl_info->_participant->create_topic(topic_name, _impl_info->_type->getName(), tqos);
+    if (_impl_info->_topic == nullptr) {
+        std::cerr << "Failed to create Topic" << std::endl;
+        return false;
+    }
+    efd::DataWriterQos wqos = efd::DATAWRITER_QOS_DEFAULT;
+    efd::DataWriterListener* listener = (efd::DataWriterListener*)_impl_info->_listener._impl.get();
+    _impl_info->_datawriter = _impl_info->_publisher->create_datawriter(_impl_info->_topic, wqos, listener);
+    if (_impl_info->_datawriter == nullptr) {
+        std::cerr << "Failed to create DataWriter" << std::endl;
+        return false;
+    }
+
+    _frame_id = _name;
+    return true;
+  }
+
+  bool CarlaISCameraPublisher::Publish() {
+    return PublishImage() && PublishInfo();
+  }
+
+  bool CarlaISCameraPublisher::PublishImage() {
+    eprosima::fastrtps::rtps::InstanceHandle_t instance_handle;
+    eprosima::fastrtps::types::ReturnCode_t rcode = _impl->_datawriter->write(&_impl->_image, instance_handle);
+    if (rcode == erc::ReturnCodeValue::RETCODE_OK) {
+        return true;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_ERROR) {
+        std::cerr << "RETCODE_ERROR" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_UNSUPPORTED) {
+        std::cerr << "RETCODE_UNSUPPORTED" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_BAD_PARAMETER) {
+        std::cerr << "RETCODE_BAD_PARAMETER" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_PRECONDITION_NOT_MET) {
+        std::cerr << "RETCODE_PRECONDITION_NOT_MET" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_OUT_OF_RESOURCES) {
+        std::cerr << "RETCODE_OUT_OF_RESOURCES" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_NOT_ENABLED) {
+        std::cerr << "RETCODE_NOT_ENABLED" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_IMMUTABLE_POLICY) {
+        std::cerr << "RETCODE_IMMUTABLE_POLICY" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_INCONSISTENT_POLICY) {
+        std::cerr << "RETCODE_INCONSISTENT_POLICY" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_ALREADY_DELETED) {
+        std::cerr << "RETCODE_ALREADY_DELETED" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_TIMEOUT) {
+        std::cerr << "RETCODE_TIMEOUT" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_NO_DATA) {
+        std::cerr << "RETCODE_NO_DATA" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_ILLEGAL_OPERATION) {
+        std::cerr << "RETCODE_ILLEGAL_OPERATION" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_NOT_ALLOWED_BY_SECURITY) {
+        std::cerr << "RETCODE_NOT_ALLOWED_BY_SECURITY" << std::endl;
+        return false;
+    }
+    std::cerr << "UNKNOWN" << std::endl;
+    return false;
+  }
+
+  bool CarlaISCameraPublisher::PublishInfo() {
+    eprosima::fastrtps::rtps::InstanceHandle_t instance_handle;
+    eprosima::fastrtps::types::ReturnCode_t rcode = _impl_info->_datawriter->write(&_impl_info->_info, instance_handle);
+    if (rcode == erc::ReturnCodeValue::RETCODE_OK) {
+        return true;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_ERROR) {
+        std::cerr << "RETCODE_ERROR" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_UNSUPPORTED) {
+        std::cerr << "RETCODE_UNSUPPORTED" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_BAD_PARAMETER) {
+        std::cerr << "RETCODE_BAD_PARAMETER" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_PRECONDITION_NOT_MET) {
+        std::cerr << "RETCODE_PRECONDITION_NOT_MET" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_OUT_OF_RESOURCES) {
+        std::cerr << "RETCODE_OUT_OF_RESOURCES" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_NOT_ENABLED) {
+        std::cerr << "RETCODE_NOT_ENABLED" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_IMMUTABLE_POLICY) {
+        std::cerr << "RETCODE_IMMUTABLE_POLICY" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_INCONSISTENT_POLICY) {
+        std::cerr << "RETCODE_INCONSISTENT_POLICY" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_ALREADY_DELETED) {
+        std::cerr << "RETCODE_ALREADY_DELETED" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_TIMEOUT) {
+        std::cerr << "RETCODE_TIMEOUT" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_NO_DATA) {
+        std::cerr << "RETCODE_NO_DATA" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_ILLEGAL_OPERATION) {
+        std::cerr << "RETCODE_ILLEGAL_OPERATION" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_NOT_ALLOWED_BY_SECURITY) {
+        std::cerr << "RETCODE_NOT_ALLOWED_BY_SECURITY" << std::endl;
+        return false;
+    }
+    std::cerr << "UNKNOWN" << std::endl;
+    return false;
+  }
+
+  void CarlaISCameraPublisher::SetImageData(int32_t seconds, uint32_t nanoseconds, size_t height, size_t width, const uint8_t* data) {
+    std::vector<uint8_t> vector_data;
+    const size_t size = height * width * 4;
+    vector_data.resize(size);
+    std::memcpy(&vector_data[0], &data[0], size);
+    SetData(seconds, nanoseconds, height, width, std::move(vector_data));
+  }
+
+  void CarlaISCameraPublisher::SetInfoRegionOfInterest( uint32_t x_offset, uint32_t y_offset, uint32_t height, uint32_t width, bool do_rectify) {
+    sensor_msgs::msg::RegionOfInterest roi;
+    roi.x_offset(x_offset);
+    roi.y_offset(y_offset);
+    roi.height(height);
+    roi.width(width);
+    roi.do_rectify(do_rectify);
+    _impl_info->_info.roi(roi);
+  }
+
+  void CarlaISCameraPublisher::SetData(int32_t seconds, uint32_t nanoseconds, size_t height, size_t width, std::vector<uint8_t>&& data) {
+    builtin_interfaces::msg::Time time;
+    time.sec(seconds);
+    time.nanosec(nanoseconds);
+
+    std_msgs::msg::Header header;
+    header.stamp(std::move(time));
+    header.frame_id(_frame_id);
+
+    _impl->_image.header(std::move(header));
+    _impl->_image.width(width);
+    _impl->_image.height(height);
+    _impl->_image.encoding("bgra8"); //taken from the list of strings in include/sensor_msgs/image_encodings.h
+    _impl->_image.is_bigendian(0);
+    _impl->_image.step(_impl->_image.width() * sizeof(uint8_t) * 4);
+    _impl->_image.data(std::move(data)); //https://github.com/eProsima/Fast-DDS/issues/2330
+  }
+
+  void CarlaISCameraPublisher::SetCameraInfoData(int32_t seconds, uint32_t nanoseconds) {
+    builtin_interfaces::msg::Time time;
+    time.sec(seconds);
+    time.nanosec(nanoseconds);
+
+    std_msgs::msg::Header header;
+    header.stamp(std::move(time));
+    header.frame_id(_frame_id);
+    _impl_info->_info.header(header);
+  }
+
+  CarlaISCameraPublisher::CarlaISCameraPublisher(const char* ros_name, const char* parent) :
+  _impl(std::make_shared<CarlaISCameraPublisherImpl>()),
+  _impl_info(std::make_shared<CarlaCameraInfoPublisherImpl>()) {
+    _name = ros_name;
+    _parent = parent;
+  }
+
+  CarlaISCameraPublisher::~CarlaISCameraPublisher() {
+      if (!_impl)
+          return;
+
+      if (_impl->_datawriter)
+          _impl->_publisher->delete_datawriter(_impl->_datawriter);
+
+      if (_impl->_publisher)
+          _impl->_participant->delete_publisher(_impl->_publisher);
+
+      if (_impl->_topic)
+          _impl->_participant->delete_topic(_impl->_topic);
+
+      if (_impl->_participant)
+          efd::DomainParticipantFactory::get_instance()->delete_participant(_impl->_participant);
+
+      if (!_impl_info)
+        return;
+
+      if (_impl_info->_datawriter)
+          _impl_info->_publisher->delete_datawriter(_impl_info->_datawriter);
+
+      if (_impl_info->_publisher)
+          _impl_info->_participant->delete_publisher(_impl_info->_publisher);
+
+      if (_impl_info->_topic)
+          _impl_info->_participant->delete_topic(_impl_info->_topic);
+
+      if (_impl_info->_participant)
+          efd::DomainParticipantFactory::get_instance()->delete_participant(_impl_info->_participant);
+  }
+
+  CarlaISCameraPublisher::CarlaISCameraPublisher(const CarlaISCameraPublisher& other) {
+    _frame_id = other._frame_id;
+    _name = other._name;
+    _parent = other._parent;
+    _impl = other._impl;
+    _impl_info = other._impl_info;
+  }
+
+  CarlaISCameraPublisher& CarlaISCameraPublisher::operator=(const CarlaISCameraPublisher& other) {
+    _frame_id = other._frame_id;
+    _name = other._name;
+    _parent = other._parent;
+    _impl = other._impl;
+    _impl_info = other._impl_info;
+
+    return *this;
+  }
+
+  CarlaISCameraPublisher::CarlaISCameraPublisher(CarlaISCameraPublisher&& other) {
+    _frame_id = std::move(other._frame_id);
+    _name = std::move(other._name);
+    _parent = std::move(other._parent);
+    _impl = std::move(other._impl);
+    _impl_info = std::move(other._impl_info);
+  }
+
+  CarlaISCameraPublisher& CarlaISCameraPublisher::operator=(CarlaISCameraPublisher&& other) {
+    _frame_id = std::move(other._frame_id);
+    _name = std::move(other._name);
+    _parent = std::move(other._parent);
+    _impl = std::move(other._impl);
+    _impl_info = std::move(other._impl_info);
+
+    return *this;
+  }
+}
+}

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

@@ -0,0 +1,51 @@
+// Copyright (c) 2022 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 <https://opensource.org/licenses/MIT>.
+
+#pragma once
+#define _GLIBCXX_USE_CXX11_ABI 0
+
+#include <memory>
+#include <vector>
+
+#include "CarlaPublisher.h"
+
+namespace carla {
+namespace ros2 {
+
+  struct CarlaISCameraPublisherImpl;
+  struct CarlaCameraInfoPublisherImpl;
+
+  class CarlaISCameraPublisher : public CarlaPublisher {
+    public:
+      CarlaISCameraPublisher(const char* ros_name = "", const char* parent = "");
+      ~CarlaISCameraPublisher();
+      CarlaISCameraPublisher(const CarlaISCameraPublisher&);
+      CarlaISCameraPublisher& operator=(const CarlaISCameraPublisher&);
+      CarlaISCameraPublisher(CarlaISCameraPublisher&&);
+      CarlaISCameraPublisher& operator=(CarlaISCameraPublisher&&);
+
+      bool Init();
+      void InitInfoData(uint32_t x_offset, uint32_t y_offset, uint32_t height, uint32_t width, float fov, bool do_rectify);
+      bool Publish();
+
+      bool HasBeenInitialized() const;
+      void SetImageData(int32_t seconds, uint32_t nanoseconds, size_t height, size_t width, const uint8_t* data);
+      void SetCameraInfoData(int32_t seconds, uint32_t nanoseconds);
+      const char* type() const override { return "instance segmentation"; }
+
+    private:
+      bool InitImage();
+      bool InitInfo();
+      bool PublishImage();
+      bool PublishInfo();
+
+      void SetInfoRegionOfInterest( uint32_t x_offset, uint32_t y_offset, uint32_t height, uint32_t width, bool do_rectify);
+      void SetData(int32_t seconds, uint32_t nanoseconds, size_t height, size_t width, std::vector<uint8_t>&& data);
+
+    private:
+      std::shared_ptr<CarlaISCameraPublisherImpl> _impl;
+      std::shared_ptr<CarlaCameraInfoPublisherImpl> _impl_info;
+  };
+}
+}

LibCarla/source/carla/ros2/publishers/CarlaLidarPublisher.cpp

@@ -0,0 +1,262 @@
+#define _GLIBCXX_USE_CXX11_ABI 0
+
+#include "CarlaLidarPublisher.h"
+
+#include <string>
+
+#include "carla/ros2/types/PointCloud2PubSubTypes.h"
+#include "carla/ros2/listeners/CarlaListener.h"
+
+#include <fastdds/dds/domain/DomainParticipant.hpp>
+#include <fastdds/dds/publisher/Publisher.hpp>
+#include <fastdds/dds/topic/Topic.hpp>
+#include <fastdds/dds/publisher/DataWriter.hpp>
+#include <fastdds/dds/topic/TypeSupport.hpp>
+
+#include <fastdds/dds/domain/qos/DomainParticipantQos.hpp>
+#include <fastdds/dds/domain/DomainParticipantFactory.hpp>
+#include <fastdds/dds/publisher/qos/PublisherQos.hpp>
+#include <fastdds/dds/topic/qos/TopicQos.hpp>
+
+#include <fastrtps/attributes/ParticipantAttributes.h>
+#include <fastrtps/qos/QosPolicies.h>
+#include <fastdds/dds/publisher/qos/DataWriterQos.hpp>
+#include <fastdds/dds/publisher/DataWriterListener.hpp>
+
+namespace carla {
+namespace ros2 {
+
+  namespace efd = eprosima::fastdds::dds;
+  using erc = eprosima::fastrtps::types::ReturnCode_t;
+
+  struct CarlaLidarPublisherImpl {
+    efd::DomainParticipant* _participant { nullptr };
+    efd::Publisher* _publisher { nullptr };
+    efd::Topic* _topic { nullptr };
+    efd::DataWriter* _datawriter { nullptr };
+    efd::TypeSupport _type { new sensor_msgs::msg::PointCloud2PubSubType() };
+    CarlaListener _listener {};
+    sensor_msgs::msg::PointCloud2 _lidar {};
+  };
+
+  bool CarlaLidarPublisher::Init() {
+    if (_impl->_type == nullptr) {
+        std::cerr << "Invalid TypeSupport" << std::endl;
+        return false;
+    }
+
+    efd::DomainParticipantQos pqos = efd::PARTICIPANT_QOS_DEFAULT;
+    pqos.name(_name);
+    auto factory = efd::DomainParticipantFactory::get_instance();
+    _impl->_participant = factory->create_participant(0, pqos);
+    if (_impl->_participant == nullptr) {
+        std::cerr << "Failed to create DomainParticipant" << std::endl;
+        return false;
+    }
+    _impl->_type.register_type(_impl->_participant);
+
+    efd::PublisherQos pubqos = efd::PUBLISHER_QOS_DEFAULT;
+    _impl->_publisher = _impl->_participant->create_publisher(pubqos, nullptr);
+    if (_impl->_publisher == nullptr) {
+      std::cerr << "Failed to create Publisher" << std::endl;
+      return false;
+    }
+
+    efd::TopicQos tqos = efd::TOPIC_QOS_DEFAULT;
+    const std::string base { "rt/carla/" };
+    std::string topic_name = base;
+    if (!_parent.empty())
+      topic_name += _parent + "/";
+    topic_name += _name;
+    _impl->_topic = _impl->_participant->create_topic(topic_name, _impl->_type->getName(), tqos);
+    if (_impl->_topic == nullptr) {
+        std::cerr << "Failed to create Topic" << std::endl;
+        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 = (efd::DataWriterListener*)_impl->_listener._impl.get();
+    _impl->_datawriter = _impl->_publisher->create_datawriter(_impl->_topic, wqos, listener);
+    if (_impl->_datawriter == nullptr) {
+        std::cerr << "Failed to create DataWriter" << std::endl;
+        return false;
+    }
+    _frame_id = _name;
+    return true;
+  }
+
+  bool CarlaLidarPublisher::Publish() {
+    eprosima::fastrtps::rtps::InstanceHandle_t instance_handle;
+    eprosima::fastrtps::types::ReturnCode_t rcode = _impl->_datawriter->write(&_impl->_lidar, instance_handle);
+    if (rcode == erc::ReturnCodeValue::RETCODE_OK) {
+        return true;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_ERROR) {
+        std::cerr << "RETCODE_ERROR" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_UNSUPPORTED) {
+        std::cerr << "RETCODE_UNSUPPORTED" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_BAD_PARAMETER) {
+        std::cerr << "RETCODE_BAD_PARAMETER" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_PRECONDITION_NOT_MET) {
+        std::cerr << "RETCODE_PRECONDITION_NOT_MET" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_OUT_OF_RESOURCES) {
+        std::cerr << "RETCODE_OUT_OF_RESOURCES" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_NOT_ENABLED) {
+        std::cerr << "RETCODE_NOT_ENABLED" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_IMMUTABLE_POLICY) {
+        std::cerr << "RETCODE_IMMUTABLE_POLICY" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_INCONSISTENT_POLICY) {
+        std::cerr << "RETCODE_INCONSISTENT_POLICY" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_ALREADY_DELETED) {
+        std::cerr << "RETCODE_ALREADY_DELETED" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_TIMEOUT) {
+        std::cerr << "RETCODE_TIMEOUT" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_NO_DATA) {
+        std::cerr << "RETCODE_NO_DATA" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_ILLEGAL_OPERATION) {
+        std::cerr << "RETCODE_ILLEGAL_OPERATION" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_NOT_ALLOWED_BY_SECURITY) {
+        std::cerr << "RETCODE_NOT_ALLOWED_BY_SECURITY" << std::endl;
+        return false;
+    }
+    std::cerr << "UNKNOWN" << std::endl;
+    return false;
+  }
+
+
+void CarlaLidarPublisher::SetData(int32_t seconds, uint32_t nanoseconds, size_t height, size_t width, float* data) {
+    float* it = data;
+    float* end = &data[height * width];
+    for (++it; it < end; it += 4) {
+        *it *= -1.0f;
+    }
+    std::vector<uint8_t> vector_data;
+    const size_t size = height * width * sizeof(float);
+    vector_data.resize(size);
+    std::memcpy(&vector_data[0], &data[0], size);
+    SetData(seconds, nanoseconds, height, width, std::move(vector_data));
+  }
+
+  void CarlaLidarPublisher::SetData(int32_t seconds, uint32_t nanoseconds, size_t height, size_t width, std::vector<uint8_t>&& data) {
+    builtin_interfaces::msg::Time time;
+    time.sec(seconds);
+    time.nanosec(nanoseconds);
+
+    std_msgs::msg::Header header;
+    header.stamp(std::move(time));
+    header.frame_id(_frame_id);
+
+    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);
+
+    const size_t point_size = 4 * sizeof(float);
+    _impl->_lidar.header(std::move(header));
+    _impl->_lidar.width(width / 4);
+    _impl->_lidar.height(height);
+    _impl->_lidar.is_bigendian(false);
+    _impl->_lidar.fields({descriptor1, descriptor2, descriptor3, descriptor4});
+    _impl->_lidar.point_step(point_size);
+    _impl->_lidar.row_step(width * sizeof(float));
+    _impl->_lidar.is_dense(false); //True if there are not invalid points
+    _impl->_lidar.data(std::move(data));
+  }
+
+  CarlaLidarPublisher::CarlaLidarPublisher(const char* ros_name, const char* parent) :
+  _impl(std::make_shared<CarlaLidarPublisherImpl>()) {
+    _name = ros_name;
+    _parent = parent;
+  }
+
+  CarlaLidarPublisher::~CarlaLidarPublisher() {
+      if (!_impl)
+          return;
+
+      if (_impl->_datawriter)
+          _impl->_publisher->delete_datawriter(_impl->_datawriter);
+
+      if (_impl->_publisher)
+          _impl->_participant->delete_publisher(_impl->_publisher);
+
+      if (_impl->_topic)
+          _impl->_participant->delete_topic(_impl->_topic);
+
+      if (_impl->_participant)
+          efd::DomainParticipantFactory::get_instance()->delete_participant(_impl->_participant);
+  }
+
+  CarlaLidarPublisher::CarlaLidarPublisher(const CarlaLidarPublisher& other) {
+    _frame_id = other._frame_id;
+    _name = other._name;
+    _parent = other._parent;
+    _impl = other._impl;
+  }
+
+  CarlaLidarPublisher& CarlaLidarPublisher::operator=(const CarlaLidarPublisher& other) {
+    _frame_id = other._frame_id;
+    _name = other._name;
+    _parent = other._parent;
+    _impl = other._impl;
+
+    return *this;
+  }
+
+  CarlaLidarPublisher::CarlaLidarPublisher(CarlaLidarPublisher&& other) {
+    _frame_id = std::move(other._frame_id);
+    _name = std::move(other._name);
+    _parent = std::move(other._parent);
+    _impl = std::move(other._impl);
+  }
+
+  CarlaLidarPublisher& CarlaLidarPublisher::operator=(CarlaLidarPublisher&& other) {
+    _frame_id = std::move(other._frame_id);
+    _name = std::move(other._name);
+    _parent = std::move(other._parent);
+    _impl = std::move(other._impl);
+
+    return *this;
+  }
+}
+}

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

@@ -0,0 +1,39 @@
+// Copyright (c) 2022 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 <https://opensource.org/licenses/MIT>.
+
+#pragma once
+#define _GLIBCXX_USE_CXX11_ABI 0
+
+#include <memory>
+#include <vector>
+
+#include "CarlaPublisher.h"
+
+namespace carla {
+namespace ros2 {
+
+  struct CarlaLidarPublisherImpl;
+
+  class CarlaLidarPublisher : public CarlaPublisher {
+    public:
+      CarlaLidarPublisher(const char* ros_name = "", const char* parent = "");
+      ~CarlaLidarPublisher();
+      CarlaLidarPublisher(const CarlaLidarPublisher&);
+      CarlaLidarPublisher& operator=(const CarlaLidarPublisher&);
+      CarlaLidarPublisher(CarlaLidarPublisher&&);
+      CarlaLidarPublisher& operator=(CarlaLidarPublisher&&);
+
+      bool Init();
+      bool Publish();
+      void SetData(int32_t seconds, uint32_t nanoseconds, size_t height, size_t width, float* data);
+      const char* type() const override { return "lidar"; }
+
+    private:
+      void SetData(int32_t seconds, uint32_t nanoseconds, size_t height, size_t width, std::vector<uint8_t>&& data);
+
+    private:
+      std::shared_ptr<CarlaLidarPublisherImpl> _impl;
+  };
+}
+}

LibCarla/source/carla/ros2/publishers/CarlaLineInvasionPublisher.cpp

@@ -0,0 +1,219 @@
+#define _GLIBCXX_USE_CXX11_ABI 0
+
+#include "CarlaLineInvasionPublisher.h"
+
+#include <string>
+
+#include "carla/ros2/types/CarlaLineInvasionPubSubTypes.h"
+#include "carla/ros2/listeners/CarlaListener.h"
+
+#include <fastdds/dds/domain/DomainParticipant.hpp>
+#include <fastdds/dds/publisher/Publisher.hpp>
+#include <fastdds/dds/topic/Topic.hpp>
+#include <fastdds/dds/publisher/DataWriter.hpp>
+#include <fastdds/dds/topic/TypeSupport.hpp>
+
+#include <fastdds/dds/domain/qos/DomainParticipantQos.hpp>
+#include <fastdds/dds/domain/DomainParticipantFactory.hpp>
+#include <fastdds/dds/publisher/qos/PublisherQos.hpp>
+#include <fastdds/dds/topic/qos/TopicQos.hpp>
+
+#include <fastrtps/attributes/ParticipantAttributes.h>
+#include <fastrtps/qos/QosPolicies.h>
+#include <fastdds/dds/publisher/qos/DataWriterQos.hpp>
+#include <fastdds/dds/publisher/DataWriterListener.hpp>
+
+
+namespace carla {
+namespace ros2 {
+  namespace efd = eprosima::fastdds::dds;
+  using erc = eprosima::fastrtps::types::ReturnCode_t;
+
+  struct CarlaLineInvasionPublisherImpl {
+    efd::DomainParticipant* _participant { nullptr };
+    efd::Publisher* _publisher { nullptr };
+    efd::Topic* _topic { nullptr };
+    efd::DataWriter* _datawriter { nullptr };
+    efd::TypeSupport _type { new carla_msgs::msg::LaneInvasionEventPubSubType() };
+    CarlaListener _listener {};
+    carla_msgs::msg::LaneInvasionEvent _event {};
+  };
+
+  bool CarlaLineInvasionPublisher::Init() {
+    if (_impl->_type == nullptr) {
+        std::cerr << "Invalid TypeSupport" << std::endl;
+        return false;
+    }
+
+    efd::DomainParticipantQos pqos = efd::PARTICIPANT_QOS_DEFAULT;
+    pqos.name(_name);
+    auto factory = efd::DomainParticipantFactory::get_instance();
+    _impl->_participant = factory->create_participant(0, pqos);
+    if (_impl->_participant == nullptr) {
+        std::cerr << "Failed to create DomainParticipant" << std::endl;
+        return false;
+    }
+    _impl->_type.register_type(_impl->_participant);
+
+    efd::PublisherQos pubqos = efd::PUBLISHER_QOS_DEFAULT;
+    _impl->_publisher = _impl->_participant->create_publisher(pubqos, nullptr);
+    if (_impl->_publisher == nullptr) {
+      std::cerr << "Failed to create Publisher" << std::endl;
+      return false;
+    }
+
+    efd::TopicQos tqos = efd::TOPIC_QOS_DEFAULT;
+    const std::string base { "rt/carla/" };
+    std::string topic_name = base;
+    if (!_parent.empty())
+      topic_name += _parent + "/";
+    topic_name += _name;
+    _impl->_topic = _impl->_participant->create_topic(topic_name, _impl->_type->getName(), tqos);
+    if (_impl->_topic == nullptr) {
+        std::cerr << "Failed to create Topic" << std::endl;
+        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 = (efd::DataWriterListener*)_impl->_listener._impl.get();
+    _impl->_datawriter = _impl->_publisher->create_datawriter(_impl->_topic, wqos, listener);
+    if (_impl->_datawriter == nullptr) {
+        std::cerr << "Failed to create DataWriter" << std::endl;
+        return false;
+    }
+    _frame_id = _name;
+    return true;
+  }
+
+  bool CarlaLineInvasionPublisher::Publish() {
+    eprosima::fastrtps::rtps::InstanceHandle_t instance_handle;
+    eprosima::fastrtps::types::ReturnCode_t rcode = _impl->_datawriter->write(&_impl->_event, instance_handle);
+    if (rcode == erc::ReturnCodeValue::RETCODE_OK) {
+        return true;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_ERROR) {
+        std::cerr << "RETCODE_ERROR" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_UNSUPPORTED) {
+        std::cerr << "RETCODE_UNSUPPORTED" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_BAD_PARAMETER) {
+        std::cerr << "RETCODE_BAD_PARAMETER" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_PRECONDITION_NOT_MET) {
+        std::cerr << "RETCODE_PRECONDITION_NOT_MET" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_OUT_OF_RESOURCES) {
+        std::cerr << "RETCODE_OUT_OF_RESOURCES" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_NOT_ENABLED) {
+        std::cerr << "RETCODE_NOT_ENABLED" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_IMMUTABLE_POLICY) {
+        std::cerr << "RETCODE_IMMUTABLE_POLICY" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_INCONSISTENT_POLICY) {
+        std::cerr << "RETCODE_INCONSISTENT_POLICY" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_ALREADY_DELETED) {
+        std::cerr << "RETCODE_ALREADY_DELETED" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_TIMEOUT) {
+        std::cerr << "RETCODE_TIMEOUT" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_NO_DATA) {
+        std::cerr << "RETCODE_NO_DATA" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_ILLEGAL_OPERATION) {
+        std::cerr << "RETCODE_ILLEGAL_OPERATION" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_NOT_ALLOWED_BY_SECURITY) {
+        std::cerr << "RETCODE_NOT_ALLOWED_BY_SECURITY" << std::endl;
+        return false;
+    }
+    std::cerr << "UNKNOWN" << std::endl;
+    return false;
+  }
+
+  void CarlaLineInvasionPublisher::SetData(int32_t seconds, uint32_t nanoseconds, const int32_t* data) {
+    builtin_interfaces::msg::Time time;
+    time.sec(seconds);
+    time.nanosec(nanoseconds);
+
+    std_msgs::msg::Header header;
+    header.stamp(std::move(time));
+    header.frame_id(_frame_id);
+
+    _impl->_event.header(std::move(header));
+    _impl->_event.crossed_lane_markings({data[0], data[1], data[2]});
+  }
+
+  CarlaLineInvasionPublisher::CarlaLineInvasionPublisher(const char* ros_name, const char* parent) :
+  _impl(std::make_shared<CarlaLineInvasionPublisherImpl>()) {
+    _name = ros_name;
+    _parent = parent;
+  }
+
+  CarlaLineInvasionPublisher::~CarlaLineInvasionPublisher() {
+      if (!_impl)
+          return;
+
+      if (_impl->_datawriter)
+          _impl->_publisher->delete_datawriter(_impl->_datawriter);
+
+      if (_impl->_publisher)
+          _impl->_participant->delete_publisher(_impl->_publisher);
+
+      if (_impl->_topic)
+          _impl->_participant->delete_topic(_impl->_topic);
+
+      if (_impl->_participant)
+          efd::DomainParticipantFactory::get_instance()->delete_participant(_impl->_participant);
+  }
+
+  CarlaLineInvasionPublisher::CarlaLineInvasionPublisher(const CarlaLineInvasionPublisher& other) {
+    _frame_id = other._frame_id;
+    _name = other._name;
+    _parent = other._parent;
+    _impl = other._impl;
+  }
+
+  CarlaLineInvasionPublisher& CarlaLineInvasionPublisher::operator=(const CarlaLineInvasionPublisher& other) {
+    _frame_id = other._frame_id;
+    _name = other._name;
+    _parent = other._parent;
+    _impl = other._impl;
+
+    return *this;
+  }
+
+  CarlaLineInvasionPublisher::CarlaLineInvasionPublisher(CarlaLineInvasionPublisher&& other) {
+    _frame_id = std::move(other._frame_id);
+    _name = std::move(other._name);
+    _parent = std::move(other._parent);
+    _impl = std::move(other._impl);
+  }
+
+  CarlaLineInvasionPublisher& CarlaLineInvasionPublisher::operator=(CarlaLineInvasionPublisher&& other) {
+    _frame_id = std::move(other._frame_id);
+    _name = std::move(other._name);
+    _parent = std::move(other._parent);
+    _impl = std::move(other._impl);
+
+    return *this;
+  }
+}
+}

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

@@ -0,0 +1,35 @@
+// Copyright (c) 2022 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 <https://opensource.org/licenses/MIT>.
+
+#pragma once
+#define _GLIBCXX_USE_CXX11_ABI 0
+
+#include <memory>
+
+#include "CarlaPublisher.h"
+
+namespace carla {
+namespace ros2 {
+
+  struct CarlaLineInvasionPublisherImpl;
+
+  class CarlaLineInvasionPublisher : public CarlaPublisher {
+    public:
+      CarlaLineInvasionPublisher(const char* ros_name = "", const char* parent = "");
+      ~CarlaLineInvasionPublisher();
+      CarlaLineInvasionPublisher(const CarlaLineInvasionPublisher&);
+      CarlaLineInvasionPublisher& operator=(const CarlaLineInvasionPublisher&);
+      CarlaLineInvasionPublisher(CarlaLineInvasionPublisher&&);
+      CarlaLineInvasionPublisher& operator=(CarlaLineInvasionPublisher&&);
+
+      bool Init();
+      bool Publish();
+      void SetData(int32_t seconds, uint32_t nanoseconds, const int32_t* data);
+      const char* type() const override { return "line invasion"; }
+
+    private:
+      std::shared_ptr<CarlaLineInvasionPublisherImpl> _impl;
+  };
+}
+}

LibCarla/source/carla/ros2/publishers/CarlaMapSensorPublisher.cpp

@@ -0,0 +1,210 @@
+#define _GLIBCXX_USE_CXX11_ABI 0
+
+#include "CarlaMapSensorPublisher.h"
+
+#include <string>
+
+#include "carla/ros2/types/StringPubSubTypes.h"
+#include "carla/ros2/listeners/CarlaListener.h"
+
+#include <fastdds/dds/domain/DomainParticipant.hpp>
+#include <fastdds/dds/publisher/Publisher.hpp>
+#include <fastdds/dds/topic/Topic.hpp>
+#include <fastdds/dds/publisher/DataWriter.hpp>
+#include <fastdds/dds/topic/TypeSupport.hpp>
+
+#include <fastdds/dds/domain/qos/DomainParticipantQos.hpp>
+#include <fastdds/dds/domain/DomainParticipantFactory.hpp>
+#include <fastdds/dds/publisher/qos/PublisherQos.hpp>
+#include <fastdds/dds/topic/qos/TopicQos.hpp>
+
+#include <fastrtps/attributes/ParticipantAttributes.h>
+#include <fastrtps/qos/QosPolicies.h>
+#include <fastdds/dds/publisher/qos/DataWriterQos.hpp>
+#include <fastdds/dds/publisher/DataWriterListener.hpp>
+
+
+namespace carla {
+namespace ros2 {
+  namespace efd = eprosima::fastdds::dds;
+  using erc = eprosima::fastrtps::types::ReturnCode_t;
+
+  struct CarlaMapSensorPublisherImpl {
+    efd::DomainParticipant* _participant { nullptr };
+    efd::Publisher* _publisher { nullptr };
+    efd::Topic* _topic { nullptr };
+    efd::DataWriter* _datawriter { nullptr };
+    efd::TypeSupport _type { new std_msgs::msg::StringPubSubType() };
+    CarlaListener _listener {};
+    std_msgs::msg::String _string {};
+  };
+
+  bool CarlaMapSensorPublisher::Init() {
+    if (_impl->_type == nullptr) {
+        std::cerr << "Invalid TypeSupport" << std::endl;
+        return false;
+    }
+
+    efd::DomainParticipantQos pqos = efd::PARTICIPANT_QOS_DEFAULT;
+    pqos.name(_name);
+    auto factory = efd::DomainParticipantFactory::get_instance();
+    _impl->_participant = factory->create_participant(0, pqos);
+    if (_impl->_participant == nullptr) {
+        std::cerr << "Failed to create DomainParticipant" << std::endl;
+        return false;
+    }
+    _impl->_type.register_type(_impl->_participant);
+
+    efd::PublisherQos pubqos = efd::PUBLISHER_QOS_DEFAULT;
+    _impl->_publisher = _impl->_participant->create_publisher(pubqos, nullptr);
+    if (_impl->_publisher == nullptr) {
+      std::cerr << "Failed to create Publisher" << std::endl;
+      return false;
+    }
+
+    efd::TopicQos tqos = efd::TOPIC_QOS_DEFAULT;
+    const std::string base { "rt/carla/" };
+    std::string topic_name = base;
+    if (!_parent.empty())
+      topic_name += _parent + "/";
+    topic_name += _name;
+    _impl->_topic = _impl->_participant->create_topic(topic_name, _impl->_type->getName(), tqos);
+    if (_impl->_topic == nullptr) {
+        std::cerr << "Failed to create Topic" << std::endl;
+        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 = (efd::DataWriterListener*)_impl->_listener._impl.get();
+    _impl->_datawriter = _impl->_publisher->create_datawriter(_impl->_topic, wqos, listener);
+    if (_impl->_datawriter == nullptr) {
+        std::cerr << "Failed to create DataWriter" << std::endl;
+        return false;
+    }
+    _frame_id = _name;
+    return true;
+  }
+
+  bool CarlaMapSensorPublisher::Publish() {
+    eprosima::fastrtps::rtps::InstanceHandle_t instance_handle;
+    eprosima::fastrtps::types::ReturnCode_t rcode = _impl->_datawriter->write(&_impl->_string, instance_handle);
+    if (rcode == erc::ReturnCodeValue::RETCODE_OK) {
+        return true;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_ERROR) {
+        std::cerr << "RETCODE_ERROR" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_UNSUPPORTED) {
+        std::cerr << "RETCODE_UNSUPPORTED" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_BAD_PARAMETER) {
+        std::cerr << "RETCODE_BAD_PARAMETER" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_PRECONDITION_NOT_MET) {
+        std::cerr << "RETCODE_PRECONDITION_NOT_MET" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_OUT_OF_RESOURCES) {
+        std::cerr << "RETCODE_OUT_OF_RESOURCES" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_NOT_ENABLED) {
+        std::cerr << "RETCODE_NOT_ENABLED" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_IMMUTABLE_POLICY) {
+        std::cerr << "RETCODE_IMMUTABLE_POLICY" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_INCONSISTENT_POLICY) {
+        std::cerr << "RETCODE_INCONSISTENT_POLICY" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_ALREADY_DELETED) {
+        std::cerr << "RETCODE_ALREADY_DELETED" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_TIMEOUT) {
+        std::cerr << "RETCODE_TIMEOUT" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_NO_DATA) {
+        std::cerr << "RETCODE_NO_DATA" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_ILLEGAL_OPERATION) {
+        std::cerr << "RETCODE_ILLEGAL_OPERATION" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_NOT_ALLOWED_BY_SECURITY) {
+        std::cerr << "RETCODE_NOT_ALLOWED_BY_SECURITY" << std::endl;
+        return false;
+    }
+    std::cerr << "UNKNOWN" << std::endl;
+    return false;
+  }
+
+  void CarlaMapSensorPublisher::SetData(const char* data) {
+    _impl->_string.data(data);
+  }
+
+  CarlaMapSensorPublisher::CarlaMapSensorPublisher(const char* ros_name, const char* parent) :
+  _impl(std::make_shared<CarlaMapSensorPublisherImpl>()) {
+    _name = ros_name;
+    _parent = parent;
+  }
+
+  CarlaMapSensorPublisher::~CarlaMapSensorPublisher() {
+      if (!_impl)
+          return;
+
+      if (_impl->_datawriter)
+          _impl->_publisher->delete_datawriter(_impl->_datawriter);
+
+      if (_impl->_publisher)
+          _impl->_participant->delete_publisher(_impl->_publisher);
+
+      if (_impl->_topic)
+          _impl->_participant->delete_topic(_impl->_topic);
+
+      if (_impl->_participant)
+          efd::DomainParticipantFactory::get_instance()->delete_participant(_impl->_participant);
+  }
+
+  CarlaMapSensorPublisher::CarlaMapSensorPublisher(const CarlaMapSensorPublisher& other) {
+    _frame_id = other._frame_id;
+    _name = other._name;
+    _parent = other._parent;
+    _impl = other._impl;
+  }
+
+  CarlaMapSensorPublisher& CarlaMapSensorPublisher::operator=(const CarlaMapSensorPublisher& other) {
+    _frame_id = other._frame_id;
+    _name = other._name;
+    _parent = other._parent;
+    _impl = other._impl;
+
+    return *this;
+  }
+
+  CarlaMapSensorPublisher::CarlaMapSensorPublisher(CarlaMapSensorPublisher&& other) {
+    _frame_id = std::move(other._frame_id);
+    _name = std::move(other._name);
+    _parent = std::move(other._parent);
+    _impl = std::move(other._impl);
+  }
+
+  CarlaMapSensorPublisher& CarlaMapSensorPublisher::operator=(CarlaMapSensorPublisher&& other) {
+    _frame_id = std::move(other._frame_id);
+    _name = std::move(other._name);
+    _parent = std::move(other._parent);
+    _impl = std::move(other._impl);
+
+    return *this;
+  }
+}
+}

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

@@ -0,0 +1,35 @@
+// Copyright (c) 2022 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 <https://opensource.org/licenses/MIT>.
+
+#pragma once
+#define _GLIBCXX_USE_CXX11_ABI 0
+
+#include <memory>
+
+#include "CarlaPublisher.h"
+
+namespace carla {
+namespace ros2 {
+
+  struct CarlaMapSensorPublisherImpl;
+
+  class CarlaMapSensorPublisher : public CarlaPublisher {
+    public:
+      CarlaMapSensorPublisher(const char* ros_name = "", const char* parent = "");
+      ~CarlaMapSensorPublisher();
+      CarlaMapSensorPublisher(const CarlaMapSensorPublisher&);
+      CarlaMapSensorPublisher& operator=(const CarlaMapSensorPublisher&);
+      CarlaMapSensorPublisher(CarlaMapSensorPublisher&&);
+      CarlaMapSensorPublisher& operator=(CarlaMapSensorPublisher&&);
+
+      bool Init();
+      bool Publish();
+      void SetData(const char* data);
+      const char* type() const override { return "map sensor"; }
+
+    private:
+      std::shared_ptr<CarlaMapSensorPublisherImpl> _impl;
+  };
+}
+}

LibCarla/source/carla/ros2/publishers/CarlaNormalsCameraPublisher.cpp

@@ -0,0 +1,416 @@
+#define _GLIBCXX_USE_CXX11_ABI 0
+
+#include "CarlaNormalsCameraPublisher.h"
+
+#include <string>
+
+#include "carla/ros2/types/ImagePubSubTypes.h"
+#include "carla/ros2/types/CameraInfoPubSubTypes.h"
+#include "carla/ros2/listeners/CarlaListener.h"
+
+#include <fastdds/dds/domain/DomainParticipant.hpp>
+#include <fastdds/dds/publisher/Publisher.hpp>
+#include <fastdds/dds/topic/Topic.hpp>
+#include <fastdds/dds/publisher/DataWriter.hpp>
+#include <fastdds/dds/topic/TypeSupport.hpp>
+
+#include <fastdds/dds/domain/qos/DomainParticipantQos.hpp>
+#include <fastdds/dds/domain/DomainParticipantFactory.hpp>
+#include <fastdds/dds/publisher/qos/PublisherQos.hpp>
+#include <fastdds/dds/topic/qos/TopicQos.hpp>
+
+#include <fastrtps/attributes/ParticipantAttributes.h>
+#include <fastrtps/qos/QosPolicies.h>
+#include <fastdds/dds/publisher/qos/DataWriterQos.hpp>
+#include <fastdds/dds/publisher/DataWriterListener.hpp>
+
+
+namespace carla {
+namespace ros2 {
+
+  namespace efd = eprosima::fastdds::dds;
+  using erc = eprosima::fastrtps::types::ReturnCode_t;
+
+  struct CarlaNormalsCameraPublisherImpl {
+    efd::DomainParticipant* _participant { nullptr };
+    efd::Publisher* _publisher { nullptr };
+    efd::Topic* _topic { nullptr };
+    efd::DataWriter* _datawriter { nullptr };
+    efd::TypeSupport _type { new sensor_msgs::msg::ImagePubSubType() };
+    CarlaListener _listener {};
+    sensor_msgs::msg::Image _image {};
+  };
+
+  struct CarlaCameraInfoPublisherImpl {
+    efd::DomainParticipant* _participant { nullptr };
+    efd::Publisher* _publisher { nullptr };
+    efd::Topic* _topic { nullptr };
+    efd::DataWriter* _datawriter { nullptr };
+    efd::TypeSupport _type { new sensor_msgs::msg::CameraInfoPubSubType() };
+    CarlaListener _listener {};
+    bool _init {false};
+    sensor_msgs::msg::CameraInfo _info {};
+  };
+
+  bool CarlaNormalsCameraPublisher::HasBeenInitialized() const {
+    return _impl_info->_init;
+  }
+
+  void CarlaNormalsCameraPublisher::InitInfoData(uint32_t x_offset, uint32_t y_offset, uint32_t height, uint32_t width, float fov, bool do_rectify) {
+    _impl_info->_info = std::move(sensor_msgs::msg::CameraInfo(height, width, fov));
+    SetInfoRegionOfInterest(x_offset, y_offset, height, width, do_rectify);
+    _impl_info->_init = true;
+  }
+
+  bool CarlaNormalsCameraPublisher::Init() {
+    return InitImage() && InitInfo();
+  }
+
+  bool CarlaNormalsCameraPublisher::InitImage() {
+    if (_impl->_type == nullptr) {
+        std::cerr << "Invalid TypeSupport" << std::endl;
+        return false;
+    }
+
+    efd::DomainParticipantQos pqos = efd::PARTICIPANT_QOS_DEFAULT;
+    pqos.name(_name);
+    auto factory = efd::DomainParticipantFactory::get_instance();
+    _impl->_participant = factory->create_participant(0, pqos);
+    if (_impl->_participant == nullptr) {
+        std::cerr << "Failed to create DomainParticipant" << std::endl;
+        return false;
+    }
+    _impl->_type.register_type(_impl->_participant);
+
+    efd::PublisherQos pubqos = efd::PUBLISHER_QOS_DEFAULT;
+    _impl->_publisher = _impl->_participant->create_publisher(pubqos, nullptr);
+    if (_impl->_publisher == nullptr) {
+      std::cerr << "Failed to create Publisher" << std::endl;
+      return false;
+    }
+
+    efd::TopicQos tqos = efd::TOPIC_QOS_DEFAULT;
+    const std::string publisher_type {"/image"};
+    const std::string base { "rt/carla/" };
+    std::string topic_name = base;
+    if (!_parent.empty())
+      topic_name += _parent + "/";
+    topic_name += _name;
+    topic_name += publisher_type;
+    _impl->_topic = _impl->_participant->create_topic(topic_name, _impl->_type->getName(), tqos);
+    if (_impl->_topic == nullptr) {
+        std::cerr << "Failed to create Topic" << std::endl;
+        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 = (efd::DataWriterListener*)_impl->_listener._impl.get();
+    _impl->_datawriter = _impl->_publisher->create_datawriter(_impl->_topic, wqos, listener);
+    if (_impl->_datawriter == nullptr) {
+        std::cerr << "Failed to create DataWriter" << std::endl;
+        return false;
+    }
+    _frame_id = _name;
+    return true;
+  }
+
+  bool CarlaNormalsCameraPublisher::InitInfo() {
+    if (_impl_info->_type == nullptr) {
+        std::cerr << "Invalid TypeSupport" << std::endl;
+        return false;
+    }
+
+    efd::DomainParticipantQos pqos = efd::PARTICIPANT_QOS_DEFAULT;
+    pqos.name(_name);
+    auto factory = efd::DomainParticipantFactory::get_instance();
+    _impl_info->_participant = factory->create_participant(0, pqos);
+    if (_impl_info->_participant == nullptr) {
+        std::cerr << "Failed to create DomainParticipant" << std::endl;
+        return false;
+    }
+    _impl_info->_type.register_type(_impl_info->_participant);
+
+    efd::PublisherQos pubqos = efd::PUBLISHER_QOS_DEFAULT;
+    _impl_info->_publisher = _impl_info->_participant->create_publisher(pubqos, nullptr);
+    if (_impl_info->_publisher == nullptr) {
+      std::cerr << "Failed to create Publisher" << std::endl;
+      return false;
+    }
+
+    efd::TopicQos tqos = efd::TOPIC_QOS_DEFAULT;
+    const std::string publisher_type {"/camera_info"};
+    const std::string base { "rt/carla/" };
+    std::string topic_name = base;
+    if (!_parent.empty())
+      topic_name += _parent + "/";
+    topic_name += _name;
+    topic_name += publisher_type;
+    _impl_info->_topic = _impl_info->_participant->create_topic(topic_name, _impl_info->_type->getName(), tqos);
+    if (_impl_info->_topic == nullptr) {
+        std::cerr << "Failed to create Topic" << std::endl;
+        return false;
+    }
+    efd::DataWriterQos wqos = efd::DATAWRITER_QOS_DEFAULT;
+    efd::DataWriterListener* listener = (efd::DataWriterListener*)_impl_info->_listener._impl.get();
+    _impl_info->_datawriter = _impl_info->_publisher->create_datawriter(_impl_info->_topic, wqos, listener);
+    if (_impl_info->_datawriter == nullptr) {
+        std::cerr << "Failed to create DataWriter" << std::endl;
+        return false;
+    }
+
+    _frame_id = _name;
+    return true;
+  }
+
+  bool CarlaNormalsCameraPublisher::Publish() {
+    return PublishImage() && PublishInfo();
+  }
+
+  bool CarlaNormalsCameraPublisher::PublishImage() {
+    eprosima::fastrtps::rtps::InstanceHandle_t instance_handle;
+    eprosima::fastrtps::types::ReturnCode_t rcode = _impl->_datawriter->write(&_impl->_image, instance_handle);
+    if (rcode == erc::ReturnCodeValue::RETCODE_OK) {
+        return true;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_ERROR) {
+        std::cerr << "RETCODE_ERROR" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_UNSUPPORTED) {
+        std::cerr << "RETCODE_UNSUPPORTED" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_BAD_PARAMETER) {
+        std::cerr << "RETCODE_BAD_PARAMETER" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_PRECONDITION_NOT_MET) {
+        std::cerr << "RETCODE_PRECONDITION_NOT_MET" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_OUT_OF_RESOURCES) {
+        std::cerr << "RETCODE_OUT_OF_RESOURCES" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_NOT_ENABLED) {
+        std::cerr << "RETCODE_NOT_ENABLED" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_IMMUTABLE_POLICY) {
+        std::cerr << "RETCODE_IMMUTABLE_POLICY" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_INCONSISTENT_POLICY) {
+        std::cerr << "RETCODE_INCONSISTENT_POLICY" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_ALREADY_DELETED) {
+        std::cerr << "RETCODE_ALREADY_DELETED" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_TIMEOUT) {
+        std::cerr << "RETCODE_TIMEOUT" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_NO_DATA) {
+        std::cerr << "RETCODE_NO_DATA" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_ILLEGAL_OPERATION) {
+        std::cerr << "RETCODE_ILLEGAL_OPERATION" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_NOT_ALLOWED_BY_SECURITY) {
+        std::cerr << "RETCODE_NOT_ALLOWED_BY_SECURITY" << std::endl;
+        return false;
+    }
+    std::cerr << "UNKNOWN" << std::endl;
+    return false;
+  }
+
+  bool CarlaNormalsCameraPublisher::PublishInfo() {
+    eprosima::fastrtps::rtps::InstanceHandle_t instance_handle;
+    eprosima::fastrtps::types::ReturnCode_t rcode = _impl_info->_datawriter->write(&_impl_info->_info, instance_handle);
+    if (rcode == erc::ReturnCodeValue::RETCODE_OK) {
+        return true;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_ERROR) {
+        std::cerr << "RETCODE_ERROR" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_UNSUPPORTED) {
+        std::cerr << "RETCODE_UNSUPPORTED" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_BAD_PARAMETER) {
+        std::cerr << "RETCODE_BAD_PARAMETER" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_PRECONDITION_NOT_MET) {
+        std::cerr << "RETCODE_PRECONDITION_NOT_MET" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_OUT_OF_RESOURCES) {
+        std::cerr << "RETCODE_OUT_OF_RESOURCES" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_NOT_ENABLED) {
+        std::cerr << "RETCODE_NOT_ENABLED" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_IMMUTABLE_POLICY) {
+        std::cerr << "RETCODE_IMMUTABLE_POLICY" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_INCONSISTENT_POLICY) {
+        std::cerr << "RETCODE_INCONSISTENT_POLICY" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_ALREADY_DELETED) {
+        std::cerr << "RETCODE_ALREADY_DELETED" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_TIMEOUT) {
+        std::cerr << "RETCODE_TIMEOUT" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_NO_DATA) {
+        std::cerr << "RETCODE_NO_DATA" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_ILLEGAL_OPERATION) {
+        std::cerr << "RETCODE_ILLEGAL_OPERATION" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_NOT_ALLOWED_BY_SECURITY) {
+        std::cerr << "RETCODE_NOT_ALLOWED_BY_SECURITY" << std::endl;
+        return false;
+    }
+    std::cerr << "UNKNOWN" << std::endl;
+    return false;
+  }
+
+  void CarlaNormalsCameraPublisher::SetImageData(int32_t seconds, uint32_t nanoseconds, size_t height, size_t width, const uint8_t* data) {    std::vector<uint8_t> vector_data;
+    const size_t size = height * width * 4;
+    vector_data.resize(size);
+    std::memcpy(&vector_data[0], &data[0], size);
+    SetData(seconds, nanoseconds,height, width, std::move(vector_data));
+  }
+
+  void CarlaNormalsCameraPublisher::SetInfoRegionOfInterest( uint32_t x_offset, uint32_t y_offset, uint32_t height, uint32_t width, bool do_rectify) {
+    sensor_msgs::msg::RegionOfInterest roi;
+    roi.x_offset(x_offset);
+    roi.y_offset(y_offset);
+    roi.height(height);
+    roi.width(width);
+    roi.do_rectify(do_rectify);
+    _impl_info->_info.roi(roi);
+  }
+
+  void CarlaNormalsCameraPublisher::SetData(int32_t seconds, uint32_t nanoseconds, size_t height, size_t width, std::vector<uint8_t>&& data) {
+    builtin_interfaces::msg::Time time;
+    time.sec(seconds);
+    time.nanosec(nanoseconds);
+
+    std_msgs::msg::Header header;
+    header.stamp(std::move(time));
+    header.frame_id(_frame_id);
+
+    _impl->_image.header(std::move(header));
+    _impl->_image.width(width);
+    _impl->_image.height(height);
+    _impl->_image.encoding("bgra8");
+    _impl->_image.is_bigendian(0);
+    _impl->_image.step(_impl->_image.width() * sizeof(uint8_t) * 4);
+    _impl->_image.data(std::move(data)); //https://github.com/eProsima/Fast-DDS/issues/2330
+  }
+
+  void CarlaNormalsCameraPublisher::SetCameraInfoData(int32_t seconds, uint32_t nanoseconds) {
+    builtin_interfaces::msg::Time time;
+    time.sec(seconds);
+    time.nanosec(nanoseconds);
+
+    std_msgs::msg::Header header;
+    header.stamp(std::move(time));
+    header.frame_id(_frame_id);
+    _impl_info->_info.header(header);
+  }
+
+  CarlaNormalsCameraPublisher::CarlaNormalsCameraPublisher(const char* ros_name, const char* parent) :
+  _impl(std::make_shared<CarlaNormalsCameraPublisherImpl>()),
+  _impl_info(std::make_shared<CarlaCameraInfoPublisherImpl>()) {
+    _name = ros_name;
+    _parent = parent;
+  }
+
+  CarlaNormalsCameraPublisher::~CarlaNormalsCameraPublisher() {
+      if (!_impl)
+          return;
+
+      if (_impl->_datawriter)
+          _impl->_publisher->delete_datawriter(_impl->_datawriter);
+
+      if (_impl->_publisher)
+          _impl->_participant->delete_publisher(_impl->_publisher);
+
+      if (_impl->_topic)
+          _impl->_participant->delete_topic(_impl->_topic);
+
+      if (_impl->_participant)
+          efd::DomainParticipantFactory::get_instance()->delete_participant(_impl->_participant);
+
+      if (!_impl_info)
+        return;
+
+      if (_impl_info->_datawriter)
+          _impl_info->_publisher->delete_datawriter(_impl_info->_datawriter);
+
+      if (_impl_info->_publisher)
+          _impl_info->_participant->delete_publisher(_impl_info->_publisher);
+
+      if (_impl_info->_topic)
+          _impl_info->_participant->delete_topic(_impl_info->_topic);
+
+      if (_impl_info->_participant)
+          efd::DomainParticipantFactory::get_instance()->delete_participant(_impl_info->_participant);
+  }
+
+  CarlaNormalsCameraPublisher::CarlaNormalsCameraPublisher(const CarlaNormalsCameraPublisher& other) {
+    _frame_id = other._frame_id;
+    _name = other._name;
+    _parent = other._parent;
+    _impl = other._impl;
+    _impl_info = other._impl_info;
+  }
+
+  CarlaNormalsCameraPublisher& CarlaNormalsCameraPublisher::operator=(const CarlaNormalsCameraPublisher& other) {
+    _frame_id = other._frame_id;
+    _name = other._name;
+    _parent = other._parent;
+    _impl = other._impl;
+    _impl_info = other._impl_info;
+
+    return *this;
+  }
+
+  CarlaNormalsCameraPublisher::CarlaNormalsCameraPublisher(CarlaNormalsCameraPublisher&& other) {
+    _frame_id = std::move(other._frame_id);
+    _name = std::move(other._name);
+    _parent = std::move(other._parent);
+    _impl = std::move(other._impl);
+    _impl_info = std::move(other._impl_info);
+
+  }
+
+  CarlaNormalsCameraPublisher& CarlaNormalsCameraPublisher::operator=(CarlaNormalsCameraPublisher&& other) {
+    _frame_id = std::move(other._frame_id);
+    _name = std::move(other._name);
+    _parent = std::move(other._parent);
+    _impl = std::move(other._impl);
+    _impl_info = std::move(other._impl_info);
+
+    return *this;
+  }
+}
+}

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

@@ -0,0 +1,52 @@
+// Copyright (c) 2022 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 <https://opensource.org/licenses/MIT>.
+
+#pragma once
+#define _GLIBCXX_USE_CXX11_ABI 0
+
+#include <memory>
+#include <vector>
+
+#include "CarlaPublisher.h"
+
+namespace carla {
+namespace ros2 {
+
+  struct CarlaNormalsCameraPublisherImpl;
+  struct CarlaCameraInfoPublisherImpl;
+
+  class CarlaNormalsCameraPublisher : public CarlaPublisher {
+    public:
+      CarlaNormalsCameraPublisher(const char* ros_name = "", const char* parent = "");
+      ~CarlaNormalsCameraPublisher();
+      CarlaNormalsCameraPublisher(const CarlaNormalsCameraPublisher&);
+      CarlaNormalsCameraPublisher& operator=(const CarlaNormalsCameraPublisher&);
+      CarlaNormalsCameraPublisher(CarlaNormalsCameraPublisher&&);
+      CarlaNormalsCameraPublisher& operator=(CarlaNormalsCameraPublisher&&);
+
+      bool Init();
+      void InitInfoData(uint32_t x_offset, uint32_t y_offset, uint32_t height, uint32_t width, float fov, bool do_rectify);
+      bool Publish();
+
+      bool HasBeenInitialized() const;
+      void SetImageData(int32_t seconds, uint32_t nanoseconds, size_t height, size_t width, const uint8_t* data);
+      void SetCameraInfoData(int32_t seconds, uint32_t nanoseconds);
+
+      const char* type() const override { return "normals camera"; }
+
+    private:
+      bool InitImage();
+      bool InitInfo();
+      bool PublishImage();
+      bool PublishInfo();
+
+      void SetInfoRegionOfInterest( uint32_t x_offset, uint32_t y_offset, uint32_t height, uint32_t width, bool do_rectify);
+      void SetData(int32_t seconds, uint32_t nanoseconds, size_t height, size_t width, std::vector<uint8_t>&& data);
+
+    private:
+      std::shared_ptr<CarlaNormalsCameraPublisherImpl> _impl;
+      std::shared_ptr<CarlaCameraInfoPublisherImpl> _impl_info;
+  };
+}
+}

LibCarla/source/carla/ros2/publishers/CarlaOpticalFlowCameraPublisher.cpp

@@ -0,0 +1,498 @@
+#define _GLIBCXX_USE_CXX11_ABI 0
+
+#include "CarlaOpticalFlowCameraPublisher.h"
+
+#include <string>
+#include <cmath>
+
+#include "carla/ros2/types/ImagePubSubTypes.h"
+#include "carla/ros2/types/CameraInfoPubSubTypes.h"
+#include "carla/ros2/listeners/CarlaListener.h"
+
+#include <fastdds/dds/domain/DomainParticipant.hpp>
+#include <fastdds/dds/publisher/Publisher.hpp>
+#include <fastdds/dds/topic/Topic.hpp>
+#include <fastdds/dds/publisher/DataWriter.hpp>
+#include <fastdds/dds/topic/TypeSupport.hpp>
+
+#include <fastdds/dds/domain/qos/DomainParticipantQos.hpp>
+#include <fastdds/dds/domain/DomainParticipantFactory.hpp>
+#include <fastdds/dds/publisher/qos/PublisherQos.hpp>
+#include <fastdds/dds/topic/qos/TopicQos.hpp>
+
+#include <fastrtps/attributes/ParticipantAttributes.h>
+#include <fastrtps/qos/QosPolicies.h>
+#include <fastdds/dds/publisher/qos/DataWriterQos.hpp>
+#include <fastdds/dds/publisher/DataWriterListener.hpp>
+
+template <typename T> T CLAMP(const T& value, const T& low, const T& high)
+{
+  return value < low ? low : (value > high ? high : value);
+}
+
+namespace carla {
+namespace ros2 {
+
+  namespace efd = eprosima::fastdds::dds;
+  using erc = eprosima::fastrtps::types::ReturnCode_t;
+  struct CarlaOpticalFlowCameraPublisherImpl {
+    efd::DomainParticipant* _participant { nullptr };
+    efd::Publisher* _publisher { nullptr };
+    efd::Topic* _topic { nullptr };
+    efd::DataWriter* _datawriter { nullptr };
+    efd::TypeSupport _type { new sensor_msgs::msg::ImagePubSubType() };
+    CarlaListener _listener {};
+    sensor_msgs::msg::Image _image {};
+  };
+
+  struct CarlaCameraInfoPublisherImpl {
+    efd::DomainParticipant* _participant { nullptr };
+    efd::Publisher* _publisher { nullptr };
+    efd::Topic* _topic { nullptr };
+    efd::DataWriter* _datawriter { nullptr };
+    efd::TypeSupport _type { new sensor_msgs::msg::CameraInfoPubSubType() };
+    CarlaListener _listener {};
+    bool _init { false};
+    sensor_msgs::msg::CameraInfo _info {};
+  };
+
+  bool CarlaOpticalFlowCameraPublisher::HasBeenInitialized() const {
+    return _impl_info->_init;
+  }
+
+  void CarlaOpticalFlowCameraPublisher::InitInfoData(uint32_t x_offset, uint32_t y_offset, uint32_t height, uint32_t width, float fov, bool do_rectify) {
+    _impl_info->_info = std::move(sensor_msgs::msg::CameraInfo(height, width, fov));
+    SetInfoRegionOfInterest(x_offset, y_offset, height, width, do_rectify);
+    _impl_info->_init = true;
+  }
+
+  bool CarlaOpticalFlowCameraPublisher::Init() {
+    return InitImage() && InitInfo();
+  }
+
+  bool CarlaOpticalFlowCameraPublisher::InitImage() {
+    if (_impl->_type == nullptr) {
+        std::cerr << "Invalid TypeSupport" << std::endl;
+        return false;
+    }
+
+    efd::DomainParticipantQos pqos = efd::PARTICIPANT_QOS_DEFAULT;
+    pqos.name(_name);
+    auto factory = efd::DomainParticipantFactory::get_instance();
+    _impl->_participant = factory->create_participant(0, pqos);
+    if (_impl->_participant == nullptr) {
+        std::cerr << "Failed to create DomainParticipant" << std::endl;
+        return false;
+    }
+    _impl->_type.register_type(_impl->_participant);
+
+    efd::PublisherQos pubqos = efd::PUBLISHER_QOS_DEFAULT;
+    _impl->_publisher = _impl->_participant->create_publisher(pubqos, nullptr);
+    if (_impl->_publisher == nullptr) {
+      std::cerr << "Failed to create Publisher" << std::endl;
+      return false;
+    }
+
+    efd::TopicQos tqos = efd::TOPIC_QOS_DEFAULT;
+    const std::string publisher_type {"/image"};
+    const std::string base { "rt/carla/" };
+    std::string topic_name = base;
+    if (!_parent.empty())
+      topic_name += _parent + "/";
+    topic_name += _name;
+    topic_name += publisher_type;
+    _impl->_topic = _impl->_participant->create_topic(topic_name, _impl->_type->getName(), tqos);
+    if (_impl->_topic == nullptr) {
+        std::cerr << "Failed to create Topic" << std::endl;
+        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 = (efd::DataWriterListener*)_impl->_listener._impl.get();
+    _impl->_datawriter = _impl->_publisher->create_datawriter(_impl->_topic, wqos, listener);
+    if (_impl->_datawriter == nullptr) {
+        std::cerr << "Failed to create DataWriter" << std::endl;
+        return false;
+    }
+    _frame_id = _name;
+    return true;
+  }
+
+  bool CarlaOpticalFlowCameraPublisher::InitInfo() {
+    if (_impl_info->_type == nullptr) {
+        std::cerr << "Invalid TypeSupport" << std::endl;
+        return false;
+    }
+
+    efd::DomainParticipantQos pqos = efd::PARTICIPANT_QOS_DEFAULT;
+    pqos.name(_name);
+    auto factory = efd::DomainParticipantFactory::get_instance();
+    _impl_info->_participant = factory->create_participant(0, pqos);
+    if (_impl_info->_participant == nullptr) {
+        std::cerr << "Failed to create DomainParticipant" << std::endl;
+        return false;
+    }
+    _impl_info->_type.register_type(_impl_info->_participant);
+
+    efd::PublisherQos pubqos = efd::PUBLISHER_QOS_DEFAULT;
+    _impl_info->_publisher = _impl_info->_participant->create_publisher(pubqos, nullptr);
+    if (_impl_info->_publisher == nullptr) {
+      std::cerr << "Failed to create Publisher" << std::endl;
+      return false;
+    }
+
+    efd::TopicQos tqos = efd::TOPIC_QOS_DEFAULT;
+    const std::string publisher_type {"/camera_info"};
+    const std::string base { "rt/carla/" };
+    std::string topic_name = base;
+    if (!_parent.empty())
+      topic_name += _parent + "/";
+    topic_name += _name;
+    topic_name += publisher_type;
+    _impl_info->_topic = _impl_info->_participant->create_topic(topic_name, _impl_info->_type->getName(), tqos);
+    if (_impl_info->_topic == nullptr) {
+        std::cerr << "Failed to create Topic" << std::endl;
+        return false;
+    }
+    efd::DataWriterQos wqos = efd::DATAWRITER_QOS_DEFAULT;
+    efd::DataWriterListener* listener = (efd::DataWriterListener*)_impl_info->_listener._impl.get();
+    _impl_info->_datawriter = _impl_info->_publisher->create_datawriter(_impl_info->_topic, wqos, listener);
+    if (_impl_info->_datawriter == nullptr) {
+        std::cerr << "Failed to create DataWriter" << std::endl;
+        return false;
+    }
+
+    _frame_id = _name;
+    return true;
+  }
+
+  bool CarlaOpticalFlowCameraPublisher::Publish() {
+    return PublishImage() && PublishInfo();
+  }
+
+  bool CarlaOpticalFlowCameraPublisher::PublishImage() {
+    eprosima::fastrtps::rtps::InstanceHandle_t instance_handle;
+    eprosima::fastrtps::types::ReturnCode_t rcode = _impl->_datawriter->write(&_impl->_image, instance_handle);
+    if (rcode == erc::ReturnCodeValue::RETCODE_OK) {
+        return true;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_ERROR) {
+        std::cerr << "RETCODE_ERROR" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_UNSUPPORTED) {
+        std::cerr << "RETCODE_UNSUPPORTED" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_BAD_PARAMETER) {
+        std::cerr << "RETCODE_BAD_PARAMETER" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_PRECONDITION_NOT_MET) {
+        std::cerr << "RETCODE_PRECONDITION_NOT_MET" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_OUT_OF_RESOURCES) {
+        std::cerr << "RETCODE_OUT_OF_RESOURCES" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_NOT_ENABLED) {
+        std::cerr << "RETCODE_NOT_ENABLED" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_IMMUTABLE_POLICY) {
+        std::cerr << "RETCODE_IMMUTABLE_POLICY" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_INCONSISTENT_POLICY) {
+        std::cerr << "RETCODE_INCONSISTENT_POLICY" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_ALREADY_DELETED) {
+        std::cerr << "RETCODE_ALREADY_DELETED" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_TIMEOUT) {
+        std::cerr << "RETCODE_TIMEOUT" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_NO_DATA) {
+        std::cerr << "RETCODE_NO_DATA" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_ILLEGAL_OPERATION) {
+        std::cerr << "RETCODE_ILLEGAL_OPERATION" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_NOT_ALLOWED_BY_SECURITY) {
+        std::cerr << "RETCODE_NOT_ALLOWED_BY_SECURITY" << std::endl;
+        return false;
+    }
+    std::cerr << "UNKNOWN" << std::endl;
+    return false;
+  }
+
+  bool CarlaOpticalFlowCameraPublisher::PublishInfo() {
+    eprosima::fastrtps::rtps::InstanceHandle_t instance_handle;
+    eprosima::fastrtps::types::ReturnCode_t rcode = _impl_info->_datawriter->write(&_impl_info->_info, instance_handle);
+    if (rcode == erc::ReturnCodeValue::RETCODE_OK) {
+        return true;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_ERROR) {
+        std::cerr << "RETCODE_ERROR" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_UNSUPPORTED) {
+        std::cerr << "RETCODE_UNSUPPORTED" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_BAD_PARAMETER) {
+        std::cerr << "RETCODE_BAD_PARAMETER" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_PRECONDITION_NOT_MET) {
+        std::cerr << "RETCODE_PRECONDITION_NOT_MET" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_OUT_OF_RESOURCES) {
+        std::cerr << "RETCODE_OUT_OF_RESOURCES" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_NOT_ENABLED) {
+        std::cerr << "RETCODE_NOT_ENABLED" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_IMMUTABLE_POLICY) {
+        std::cerr << "RETCODE_IMMUTABLE_POLICY" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_INCONSISTENT_POLICY) {
+        std::cerr << "RETCODE_INCONSISTENT_POLICY" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_ALREADY_DELETED) {
+        std::cerr << "RETCODE_ALREADY_DELETED" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_TIMEOUT) {
+        std::cerr << "RETCODE_TIMEOUT" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_NO_DATA) {
+        std::cerr << "RETCODE_NO_DATA" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_ILLEGAL_OPERATION) {
+        std::cerr << "RETCODE_ILLEGAL_OPERATION" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_NOT_ALLOWED_BY_SECURITY) {
+        std::cerr << "RETCODE_NOT_ALLOWED_BY_SECURITY" << std::endl;
+        return false;
+    }
+    std::cerr << "UNKNOWN" << std::endl;
+    return false;
+  }
+
+  void CarlaOpticalFlowCameraPublisher::SetImageData(int32_t seconds, uint32_t nanoseconds, size_t height, size_t width, const float* data) {
+    constexpr float pi = 3.1415f;
+    constexpr float rad2ang = 360.0f/(2.0f*pi);
+    const size_t max_index = width * height * 2;
+    std::vector<uint8_t> vector_data;
+    vector_data.resize(height * width * 4);
+    size_t data_index = 0;
+    for (size_t index = 0; index < max_index; index += 2) {
+        const float vx = data[index];
+        const float vy = data[index + 1];
+        float angle = 180.0f + std::atan2(vy, vx) * rad2ang;
+        if (angle < 0)
+        {
+            angle = 360.0f + angle;
+        }
+        angle = std::fmod(angle, 360.0f);
+
+        const float norm = std::sqrt(vx * vx + vy * vy);
+        const float shift = 0.999f;
+        const float a = 1.0f / std::log(0.1f + shift);
+        const float intensity = CLAMP<float>(a * std::log(norm + shift), 0.0f, 1.0f);
+
+        const float& H = angle;
+        const float S = 1.0f;
+        const float V = intensity;
+        const float H_60 = H * (1.0f / 60.0f);
+
+        const float C = V * S;
+        const float X = C * (1.0f - std::abs(std::fmod(H_60, 2.0f) - 1.0f));
+        const float m = V - C;
+
+        float r = 0;
+        float g = 0;
+        float b = 0;
+        const unsigned int angle_case = static_cast<const unsigned int>(H_60);
+        switch (angle_case) {
+        case 0:
+            r = C;
+            g = X;
+            b = 0;
+            break;
+        case 1:
+            r = X;
+            g = C;
+            b = 0;
+            break;
+        case 2:
+            r = 0;
+            g = C;
+            b = X;
+            break;
+        case 3:
+            r = 0;
+            g = X;
+            b = C;
+            break;
+        case 4:
+            r = X;
+            g = 0;
+            b = C;
+            break;
+        case 5:
+            r = C;
+            g = 0;
+            b = X;
+            break;
+        default:
+            r = 1;
+            g = 1;
+            b = 1;
+            break;
+        }
+
+        const uint8_t R = static_cast<uint8_t>((r + m) * 255.0f);
+        const uint8_t G = static_cast<uint8_t>((g + m) * 255.0f);
+        const uint8_t B = static_cast<uint8_t>((b + m) * 255.0f);
+
+        vector_data[data_index++] = B;
+        vector_data[data_index++] = G;
+        vector_data[data_index++] = R;
+        vector_data[data_index++] = 0;
+    }
+    SetData(seconds, nanoseconds, height, width, std::move(vector_data));
+  }
+
+  void CarlaOpticalFlowCameraPublisher::SetInfoRegionOfInterest( uint32_t x_offset, uint32_t y_offset, uint32_t height, uint32_t width, bool do_rectify) {
+    sensor_msgs::msg::RegionOfInterest roi;
+    roi.x_offset(x_offset);
+    roi.y_offset(y_offset);
+    roi.height(height);
+    roi.width(width);
+    roi.do_rectify(do_rectify);
+    _impl_info->_info.roi(roi);
+  }
+
+  void CarlaOpticalFlowCameraPublisher::SetData(int32_t seconds, uint32_t nanoseconds, size_t height, size_t width, std::vector<uint8_t>&& data) {
+    builtin_interfaces::msg::Time time;
+    time.sec(seconds);
+    time.nanosec(nanoseconds);
+
+    std_msgs::msg::Header header;
+    header.stamp(std::move(time));
+    header.frame_id(_frame_id);
+
+    _impl->_image.header(std::move(header));
+    _impl->_image.width(width);
+    _impl->_image.height(height);
+    _impl->_image.encoding("bgra8");
+    _impl->_image.is_bigendian(0);
+    _impl->_image.step(_impl->_image.width() * sizeof(uint8_t) * 4);
+    _impl->_image.data(std::move(data)); //https://github.com/eProsima/Fast-DDS/issues/2330
+  }
+
+  void CarlaOpticalFlowCameraPublisher::SetCameraInfoData(int32_t seconds, uint32_t nanoseconds) {
+    builtin_interfaces::msg::Time time;
+    time.sec(seconds);
+    time.nanosec(nanoseconds);
+
+    std_msgs::msg::Header header;
+    header.stamp(std::move(time));
+    header.frame_id(_frame_id);
+    _impl_info->_info.header(header);
+  }
+
+  CarlaOpticalFlowCameraPublisher::CarlaOpticalFlowCameraPublisher(const char* ros_name, const char* parent) :
+  _impl(std::make_shared<CarlaOpticalFlowCameraPublisherImpl>()),
+  _impl_info(std::make_shared<CarlaCameraInfoPublisherImpl>()) {
+    _name = ros_name;
+    _parent = parent;
+  }
+
+  CarlaOpticalFlowCameraPublisher::~CarlaOpticalFlowCameraPublisher() {
+      if (!_impl)
+          return;
+
+      if (_impl->_datawriter)
+          _impl->_publisher->delete_datawriter(_impl->_datawriter);
+
+      if (_impl->_publisher)
+          _impl->_participant->delete_publisher(_impl->_publisher);
+
+      if (_impl->_topic)
+          _impl->_participant->delete_topic(_impl->_topic);
+
+      if (_impl->_participant)
+          efd::DomainParticipantFactory::get_instance()->delete_participant(_impl->_participant);
+
+      if (!_impl_info)
+        return;
+
+      if (_impl_info->_datawriter)
+          _impl_info->_publisher->delete_datawriter(_impl_info->_datawriter);
+
+      if (_impl_info->_publisher)
+          _impl_info->_participant->delete_publisher(_impl_info->_publisher);
+
+      if (_impl_info->_topic)
+          _impl_info->_participant->delete_topic(_impl_info->_topic);
+
+      if (_impl_info->_participant)
+          efd::DomainParticipantFactory::get_instance()->delete_participant(_impl_info->_participant);
+  }
+
+  CarlaOpticalFlowCameraPublisher::CarlaOpticalFlowCameraPublisher(const CarlaOpticalFlowCameraPublisher& other) {
+    _frame_id = other._frame_id;
+    _name = other._name;
+    _parent = other._parent;
+    _impl = other._impl;
+    _impl_info = other._impl_info;
+  }
+
+  CarlaOpticalFlowCameraPublisher& CarlaOpticalFlowCameraPublisher::operator=(const CarlaOpticalFlowCameraPublisher& other) {
+    _frame_id = other._frame_id;
+    _name = other._name;
+    _parent = other._parent;
+    _impl = other._impl;
+    _impl_info = other._impl_info;
+
+    return *this;
+  }
+
+  CarlaOpticalFlowCameraPublisher::CarlaOpticalFlowCameraPublisher(CarlaOpticalFlowCameraPublisher&& other) {
+    _frame_id = std::move(other._frame_id);
+    _name = std::move(other._name);
+    _parent = std::move(other._parent);
+    _impl = std::move(other._impl);
+    _impl_info = std::move(other._impl_info);
+
+  }
+
+  CarlaOpticalFlowCameraPublisher& CarlaOpticalFlowCameraPublisher::operator=(CarlaOpticalFlowCameraPublisher&& other) {
+    _frame_id = std::move(other._frame_id);
+    _name = std::move(other._name);
+    _parent = std::move(other._parent);
+    _impl = std::move(other._impl);
+    _impl_info = std::move(other._impl_info);
+
+    return *this;
+  }
+}
+}

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

@@ -0,0 +1,51 @@
+// Copyright (c) 2022 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 <https://opensource.org/licenses/MIT>.
+
+#pragma once
+#define _GLIBCXX_USE_CXX11_ABI 0
+
+#include <memory>
+#include <vector>
+
+#include "CarlaPublisher.h"
+
+namespace carla {
+namespace ros2 {
+
+  struct CarlaOpticalFlowCameraPublisherImpl;
+  struct CarlaCameraInfoPublisherImpl;
+
+  class CarlaOpticalFlowCameraPublisher : public CarlaPublisher {
+    public:
+      CarlaOpticalFlowCameraPublisher(const char* ros_name = "", const char* parent = "");
+      ~CarlaOpticalFlowCameraPublisher();
+      CarlaOpticalFlowCameraPublisher(const CarlaOpticalFlowCameraPublisher&);
+      CarlaOpticalFlowCameraPublisher& operator=(const CarlaOpticalFlowCameraPublisher&);
+      CarlaOpticalFlowCameraPublisher(CarlaOpticalFlowCameraPublisher&&);
+      CarlaOpticalFlowCameraPublisher& operator=(CarlaOpticalFlowCameraPublisher&&);
+
+      bool Init();
+      void InitInfoData(uint32_t x_offset, uint32_t y_offset, uint32_t height, uint32_t width, float fov, bool do_rectify);
+      bool Publish();
+
+      bool HasBeenInitialized() const;
+      void SetImageData(int32_t seconds, uint32_t nanoseconds, size_t height, size_t width, const float* data);
+      void SetCameraInfoData(int32_t seconds, uint32_t nanoseconds);
+      const char* type() const override { return "optical flow camera"; }
+
+    private:
+      bool InitImage();
+      bool InitInfo();
+      bool PublishImage();
+      bool PublishInfo();
+
+      void SetInfoRegionOfInterest( uint32_t x_offset, uint32_t y_offset, uint32_t height, uint32_t width, bool do_rectify);
+      void SetData(int32_t seconds, uint32_t nanoseconds, size_t height, size_t width, std::vector<uint8_t>&& data);
+
+    private:
+      std::shared_ptr<CarlaOpticalFlowCameraPublisherImpl> _impl;
+      std::shared_ptr<CarlaCameraInfoPublisherImpl> _impl_info;
+  };
+}
+}

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

@@ -0,0 +1,35 @@
+// Copyright (c) 2022 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 <https://opensource.org/licenses/MIT>.
+
+#pragma once
+#define _GLIBCXX_USE_CXX11_ABI 0
+
+#include <string>
+
+namespace carla {
+namespace ros2 {
+
+  class CarlaPublisher {
+    public:
+      const std::string& frame_id() const { return _frame_id; }
+      const std::string& name() const { return _name; }
+      const std::string& parent() const { return _parent; }
+
+      void frame_id(std::string&& frame_id) { _frame_id = std::move(frame_id); }
+      void name(std::string&& name) { _name = std::move(name); }
+      void parent(std::string&& parent) { _parent = std::move(parent); }
+
+      virtual const char* type() const = 0;
+
+    public:
+      CarlaPublisher() = default;
+      virtual ~CarlaPublisher() = default;
+
+    protected:
+      std::string _frame_id = "";
+      std::string _name = "";
+      std::string _parent = "";
+  };
+}
+}

LibCarla/source/carla/ros2/publishers/CarlaRGBCameraPublisher.cpp

@@ -0,0 +1,417 @@
+#define _GLIBCXX_USE_CXX11_ABI 0
+
+#include "CarlaRGBCameraPublisher.h"
+
+#include <string>
+
+#include "carla/ros2/types/ImagePubSubTypes.h"
+#include "carla/ros2/types/CameraInfoPubSubTypes.h"
+#include "carla/ros2/listeners/CarlaListener.h"
+
+#include <fastdds/dds/domain/DomainParticipant.hpp>
+#include <fastdds/dds/publisher/Publisher.hpp>
+#include <fastdds/dds/topic/Topic.hpp>
+#include <fastdds/dds/publisher/DataWriter.hpp>
+#include <fastdds/dds/topic/TypeSupport.hpp>
+
+#include <fastdds/dds/domain/qos/DomainParticipantQos.hpp>
+#include <fastdds/dds/domain/DomainParticipantFactory.hpp>
+#include <fastdds/dds/publisher/qos/PublisherQos.hpp>
+#include <fastdds/dds/topic/qos/TopicQos.hpp>
+
+#include <fastrtps/attributes/ParticipantAttributes.h>
+#include <fastrtps/qos/QosPolicies.h>
+#include <fastdds/dds/publisher/qos/DataWriterQos.hpp>
+#include <fastdds/dds/publisher/DataWriterListener.hpp>
+
+
+namespace carla {
+namespace ros2 {
+
+  namespace efd = eprosima::fastdds::dds;
+  using erc = eprosima::fastrtps::types::ReturnCode_t;
+
+  struct CarlaRGBCameraPublisherImpl {
+    efd::DomainParticipant* _participant { nullptr };
+    efd::Publisher* _publisher { nullptr };
+    efd::Topic* _topic { nullptr };
+    efd::DataWriter* _datawriter { nullptr };
+    efd::TypeSupport _type { new sensor_msgs::msg::ImagePubSubType() };
+    CarlaListener _listener {};
+    sensor_msgs::msg::Image _image {};
+  };
+
+  struct CarlaCameraInfoPublisherImpl {
+    efd::DomainParticipant* _participant { nullptr };
+    efd::Publisher* _publisher { nullptr };
+    efd::Topic* _topic { nullptr };
+    efd::DataWriter* _datawriter { nullptr };
+    efd::TypeSupport _type { new sensor_msgs::msg::CameraInfoPubSubType() };
+    CarlaListener _listener {};
+    bool _init {false};
+    sensor_msgs::msg::CameraInfo _info {};
+  };
+
+  bool CarlaRGBCameraPublisher::HasBeenInitialized() const {
+    return _impl_info->_init;
+  }
+
+  void CarlaRGBCameraPublisher::InitInfoData(uint32_t x_offset, uint32_t y_offset, uint32_t height, uint32_t width, float fov, bool do_rectify) {
+    _impl_info->_info = std::move(sensor_msgs::msg::CameraInfo(height, width, fov));
+    SetInfoRegionOfInterest(x_offset, y_offset, height, width, do_rectify);
+    _impl_info->_init = true;
+  }
+
+  bool CarlaRGBCameraPublisher::Init() {
+    return InitImage() && InitInfo();
+  }
+
+  bool CarlaRGBCameraPublisher::InitImage() {
+    if (_impl->_type == nullptr) {
+        std::cerr << "Invalid TypeSupport" << std::endl;
+        return false;
+    }
+
+    efd::DomainParticipantQos pqos = efd::PARTICIPANT_QOS_DEFAULT;
+    pqos.name(_name);
+    auto factory = efd::DomainParticipantFactory::get_instance();
+    _impl->_participant = factory->create_participant(0, pqos);
+    if (_impl->_participant == nullptr) {
+        std::cerr << "Failed to create DomainParticipant" << std::endl;
+        return false;
+    }
+    _impl->_type.register_type(_impl->_participant);
+
+    efd::PublisherQos pubqos = efd::PUBLISHER_QOS_DEFAULT;
+    _impl->_publisher = _impl->_participant->create_publisher(pubqos, nullptr);
+    if (_impl->_publisher == nullptr) {
+      std::cerr << "Failed to create Publisher" << std::endl;
+      return false;
+    }
+
+    efd::TopicQos tqos = efd::TOPIC_QOS_DEFAULT;
+    const std::string publisher_type {"/image"};
+    const std::string base { "rt/carla/" };
+    std::string topic_name = base;
+    if (!_parent.empty())
+      topic_name += _parent + "/";
+    topic_name += _name;
+    topic_name += publisher_type;
+    _impl->_topic = _impl->_participant->create_topic(topic_name, _impl->_type->getName(), tqos);
+    if (_impl->_topic == nullptr) {
+        std::cerr << "Failed to create Topic" << std::endl;
+        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 = (efd::DataWriterListener*)_impl->_listener._impl.get();
+    _impl->_datawriter = _impl->_publisher->create_datawriter(_impl->_topic, wqos, listener);
+    if (_impl->_datawriter == nullptr) {
+        std::cerr << "Failed to create DataWriter" << std::endl;
+        return false;
+    }
+
+    _frame_id = _name;
+    return true;
+  }
+
+  bool CarlaRGBCameraPublisher::InitInfo() {
+    if (_impl_info->_type == nullptr) {
+        std::cerr << "Invalid TypeSupport" << std::endl;
+        return false;
+    }
+
+    efd::DomainParticipantQos pqos = efd::PARTICIPANT_QOS_DEFAULT;
+    pqos.name(_name);
+    auto factory = efd::DomainParticipantFactory::get_instance();
+    _impl_info->_participant = factory->create_participant(0, pqos);
+    if (_impl_info->_participant == nullptr) {
+        std::cerr << "Failed to create DomainParticipant" << std::endl;
+        return false;
+    }
+    _impl_info->_type.register_type(_impl_info->_participant);
+
+    efd::PublisherQos pubqos = efd::PUBLISHER_QOS_DEFAULT;
+    _impl_info->_publisher = _impl_info->_participant->create_publisher(pubqos, nullptr);
+    if (_impl_info->_publisher == nullptr) {
+      std::cerr << "Failed to create Publisher" << std::endl;
+      return false;
+    }
+
+    efd::TopicQos tqos = efd::TOPIC_QOS_DEFAULT;
+    const std::string publisher_type {"/camera_info"};
+    const std::string base { "rt/carla/" };
+    std::string topic_name = base;
+    if (!_parent.empty())
+      topic_name += _parent + "/";
+    topic_name += _name;
+    topic_name += publisher_type;
+    _impl_info->_topic = _impl_info->_participant->create_topic(topic_name, _impl_info->_type->getName(), tqos);
+    if (_impl_info->_topic == nullptr) {
+        std::cerr << "Failed to create Topic" << std::endl;
+        return false;
+    }
+    efd::DataWriterQos wqos = efd::DATAWRITER_QOS_DEFAULT;
+    efd::DataWriterListener* listener = (efd::DataWriterListener*)_impl_info->_listener._impl.get();
+    _impl_info->_datawriter = _impl_info->_publisher->create_datawriter(_impl_info->_topic, wqos, listener);
+    if (_impl_info->_datawriter == nullptr) {
+        std::cerr << "Failed to create DataWriter" << std::endl;
+        return false;
+    }
+
+    _frame_id = _name;
+    return true;
+  }
+
+  bool CarlaRGBCameraPublisher::Publish() {
+    return PublishImage() && PublishInfo();
+  }
+
+  bool CarlaRGBCameraPublisher::PublishImage() {
+    eprosima::fastrtps::rtps::InstanceHandle_t instance_handle;
+    eprosima::fastrtps::types::ReturnCode_t rcode = _impl->_datawriter->write(&_impl->_image, instance_handle);
+    if (rcode == erc::ReturnCodeValue::RETCODE_OK) {
+        return true;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_ERROR) {
+        std::cerr << "RETCODE_ERROR" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_UNSUPPORTED) {
+        std::cerr << "RETCODE_UNSUPPORTED" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_BAD_PARAMETER) {
+        std::cerr << "RETCODE_BAD_PARAMETER" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_PRECONDITION_NOT_MET) {
+        std::cerr << "RETCODE_PRECONDITION_NOT_MET" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_OUT_OF_RESOURCES) {
+        std::cerr << "RETCODE_OUT_OF_RESOURCES" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_NOT_ENABLED) {
+        std::cerr << "RETCODE_NOT_ENABLED" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_IMMUTABLE_POLICY) {
+        std::cerr << "RETCODE_IMMUTABLE_POLICY" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_INCONSISTENT_POLICY) {
+        std::cerr << "RETCODE_INCONSISTENT_POLICY" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_ALREADY_DELETED) {
+        std::cerr << "RETCODE_ALREADY_DELETED" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_TIMEOUT) {
+        std::cerr << "RETCODE_TIMEOUT" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_NO_DATA) {
+        std::cerr << "RETCODE_NO_DATA" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_ILLEGAL_OPERATION) {
+        std::cerr << "RETCODE_ILLEGAL_OPERATION" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_NOT_ALLOWED_BY_SECURITY) {
+        std::cerr << "RETCODE_NOT_ALLOWED_BY_SECURITY" << std::endl;
+        return false;
+    }
+    std::cerr << "UNKNOWN" << std::endl;
+    return false;
+  }
+
+  bool CarlaRGBCameraPublisher::PublishInfo() {
+    eprosima::fastrtps::rtps::InstanceHandle_t instance_handle;
+    eprosima::fastrtps::types::ReturnCode_t rcode = _impl_info->_datawriter->write(&_impl_info->_info, instance_handle);
+    if (rcode == erc::ReturnCodeValue::RETCODE_OK) {
+        return true;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_ERROR) {
+        std::cerr << "RETCODE_ERROR" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_UNSUPPORTED) {
+        std::cerr << "RETCODE_UNSUPPORTED" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_BAD_PARAMETER) {
+        std::cerr << "RETCODE_BAD_PARAMETER" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_PRECONDITION_NOT_MET) {
+        std::cerr << "RETCODE_PRECONDITION_NOT_MET" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_OUT_OF_RESOURCES) {
+        std::cerr << "RETCODE_OUT_OF_RESOURCES" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_NOT_ENABLED) {
+        std::cerr << "RETCODE_NOT_ENABLED" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_IMMUTABLE_POLICY) {
+        std::cerr << "RETCODE_IMMUTABLE_POLICY" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_INCONSISTENT_POLICY) {
+        std::cerr << "RETCODE_INCONSISTENT_POLICY" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_ALREADY_DELETED) {
+        std::cerr << "RETCODE_ALREADY_DELETED" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_TIMEOUT) {
+        std::cerr << "RETCODE_TIMEOUT" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_NO_DATA) {
+        std::cerr << "RETCODE_NO_DATA" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_ILLEGAL_OPERATION) {
+        std::cerr << "RETCODE_ILLEGAL_OPERATION" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_NOT_ALLOWED_BY_SECURITY) {
+        std::cerr << "RETCODE_NOT_ALLOWED_BY_SECURITY" << std::endl;
+        return false;
+    }
+    std::cerr << "UNKNOWN" << std::endl;
+    return false;
+  }
+
+void CarlaRGBCameraPublisher::SetImageData(int32_t seconds, uint32_t nanoseconds, uint32_t height, uint32_t width, const uint8_t* data) {
+    std::vector<uint8_t> vector_data;
+    const size_t size = height * width * 4;
+    vector_data.resize(size);
+    std::memcpy(&vector_data[0], &data[0], size);
+    SetImageData(seconds, nanoseconds, height, width, std::move(vector_data));
+  }
+
+  void CarlaRGBCameraPublisher::SetImageData(int32_t seconds, uint32_t nanoseconds, uint32_t height, uint32_t width, std::vector<uint8_t>&& data) {
+
+    builtin_interfaces::msg::Time time;
+    time.sec(seconds);
+    time.nanosec(nanoseconds);
+
+    std_msgs::msg::Header header;
+    header.stamp(std::move(time));
+    header.frame_id(_frame_id);
+    _impl->_image.header(header);
+
+    _impl->_image.width(width);
+    _impl->_image.height(height);
+    _impl->_image.encoding("bgra8");
+    _impl->_image.is_bigendian(0);
+    _impl->_image.step(_impl->_image.width() * sizeof(uint8_t) * 4);
+    _impl->_image.data(std::move(data));
+  }
+
+  void CarlaRGBCameraPublisher::SetCameraInfoData(int32_t seconds, uint32_t nanoseconds) {
+    builtin_interfaces::msg::Time time;
+    time.sec(seconds);
+    time.nanosec(nanoseconds);
+
+    std_msgs::msg::Header header;
+    header.stamp(std::move(time));
+    header.frame_id(_frame_id);
+    _impl_info->_info.header(header);
+  }
+
+  void CarlaRGBCameraPublisher::SetInfoRegionOfInterest( uint32_t x_offset, uint32_t y_offset, uint32_t height, uint32_t width, bool do_rectify) {
+    sensor_msgs::msg::RegionOfInterest roi;
+    roi.x_offset(x_offset);
+    roi.y_offset(y_offset);
+    roi.height(height);
+    roi.width(width);
+    roi.do_rectify(do_rectify);
+    _impl_info->_info.roi(roi);
+  }
+
+  CarlaRGBCameraPublisher::CarlaRGBCameraPublisher(const char* ros_name, const char* parent) :
+  _impl(std::make_shared<CarlaRGBCameraPublisherImpl>()),
+  _impl_info(std::make_shared<CarlaCameraInfoPublisherImpl>()) {
+    _name = ros_name;
+    _parent = parent;
+  }
+
+  CarlaRGBCameraPublisher::~CarlaRGBCameraPublisher() {
+      if (!_impl)
+          return;
+
+      if (_impl->_datawriter)
+          _impl->_publisher->delete_datawriter(_impl->_datawriter);
+
+      if (_impl->_publisher)
+          _impl->_participant->delete_publisher(_impl->_publisher);
+
+      if (_impl->_topic)
+          _impl->_participant->delete_topic(_impl->_topic);
+
+      if (_impl->_participant)
+          efd::DomainParticipantFactory::get_instance()->delete_participant(_impl->_participant);
+
+      if (!_impl_info)
+        return;
+
+      if (_impl_info->_datawriter)
+          _impl_info->_publisher->delete_datawriter(_impl_info->_datawriter);
+
+      if (_impl_info->_publisher)
+          _impl_info->_participant->delete_publisher(_impl_info->_publisher);
+
+      if (_impl_info->_topic)
+          _impl_info->_participant->delete_topic(_impl_info->_topic);
+
+      if (_impl_info->_participant)
+          efd::DomainParticipantFactory::get_instance()->delete_participant(_impl_info->_participant);
+  }
+
+  CarlaRGBCameraPublisher::CarlaRGBCameraPublisher(const CarlaRGBCameraPublisher& other) {
+    _frame_id = other._frame_id;
+    _name = other._name;
+    _parent = other._parent;
+    _impl = other._impl;
+    _impl_info = other._impl_info;
+  }
+
+  CarlaRGBCameraPublisher& CarlaRGBCameraPublisher::operator=(const CarlaRGBCameraPublisher& other) {
+    _frame_id = other._frame_id;
+    _name = other._name;
+    _parent = other._parent;
+    _impl = other._impl;
+    _impl_info = other._impl_info;
+
+    return *this;
+  }
+
+  CarlaRGBCameraPublisher::CarlaRGBCameraPublisher(CarlaRGBCameraPublisher&& other) {
+    _frame_id = std::move(other._frame_id);
+    _name = std::move(other._name);
+    _parent = std::move(other._parent);
+    _impl = std::move(other._impl);
+    _impl_info = std::move(other._impl_info);
+  }
+
+  CarlaRGBCameraPublisher& CarlaRGBCameraPublisher::operator=(CarlaRGBCameraPublisher&& other) {
+    _frame_id = std::move(other._frame_id);
+    _name = std::move(other._name);
+    _parent = std::move(other._parent);
+    _impl = std::move(other._impl);
+    _impl_info = std::move(other._impl_info);
+
+    return *this;
+  }
+}
+}

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

@@ -0,0 +1,51 @@
+// Copyright (c) 2022 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 <https://opensource.org/licenses/MIT>.
+
+#pragma once
+#define _GLIBCXX_USE_CXX11_ABI 0
+
+#include <memory>
+#include <vector>
+
+#include "CarlaPublisher.h"
+
+namespace carla {
+namespace ros2 {
+
+  struct CarlaRGBCameraPublisherImpl;
+  struct CarlaCameraInfoPublisherImpl;
+
+  class CarlaRGBCameraPublisher : public CarlaPublisher {
+    public:
+      CarlaRGBCameraPublisher(const char* ros_name = "", const char* parent = "");
+      ~CarlaRGBCameraPublisher();
+      CarlaRGBCameraPublisher(const CarlaRGBCameraPublisher&);
+      CarlaRGBCameraPublisher& operator=(const CarlaRGBCameraPublisher&);
+      CarlaRGBCameraPublisher(CarlaRGBCameraPublisher&&);
+      CarlaRGBCameraPublisher& operator=(CarlaRGBCameraPublisher&&);
+
+      bool Init();
+      void InitInfoData(uint32_t x_offset, uint32_t y_offset, uint32_t height, uint32_t width, float fov, bool do_rectify);
+      bool Publish();
+
+      bool HasBeenInitialized() const;
+      void SetImageData(int32_t seconds, uint32_t nanoseconds, uint32_t height, uint32_t width, const uint8_t* data);
+      void SetCameraInfoData(int32_t seconds, uint32_t nanoseconds);
+      const char* type() const override { return "rgb camera"; }
+
+    private:
+      bool InitImage();
+      bool InitInfo();
+      bool PublishImage();
+      bool PublishInfo();
+
+      void SetImageData(int32_t seconds, uint32_t nanoseconds, uint32_t height, uint32_t width, std::vector<uint8_t>&& data);
+      void SetInfoRegionOfInterest(uint32_t x_offset, uint32_t y_offset, uint32_t height, uint32_t width, bool do_rectify);
+
+    private:
+      std::shared_ptr<CarlaRGBCameraPublisherImpl> _impl;
+      std::shared_ptr<CarlaCameraInfoPublisherImpl> _impl_info;
+  };
+}
+}

LibCarla/source/carla/ros2/publishers/CarlaRadarPublisher.cpp

@@ -0,0 +1,289 @@
+#define _GLIBCXX_USE_CXX11_ABI 0
+
+#include "CarlaRadarPublisher.h"
+
+#include <string>
+
+#include "carla/sensor/data/RadarData.h"
+#include "carla/ros2/types/PointCloud2PubSubTypes.h"
+#include "carla/ros2/listeners/CarlaListener.h"
+
+#include <fastdds/dds/domain/DomainParticipant.hpp>
+#include <fastdds/dds/publisher/Publisher.hpp>
+#include <fastdds/dds/topic/Topic.hpp>
+#include <fastdds/dds/publisher/DataWriter.hpp>
+#include <fastdds/dds/topic/TypeSupport.hpp>
+
+#include <fastdds/dds/domain/qos/DomainParticipantQos.hpp>
+#include <fastdds/dds/domain/DomainParticipantFactory.hpp>
+#include <fastdds/dds/publisher/qos/PublisherQos.hpp>
+#include <fastdds/dds/topic/qos/TopicQos.hpp>
+
+#include <fastrtps/attributes/ParticipantAttributes.h>
+#include <fastrtps/qos/QosPolicies.h>
+#include <fastdds/dds/publisher/qos/DataWriterQos.hpp>
+#include <fastdds/dds/publisher/DataWriterListener.hpp>
+
+
+namespace carla {
+namespace ros2 {
+
+  namespace efd = eprosima::fastdds::dds;
+  using erc = eprosima::fastrtps::types::ReturnCode_t;
+
+  struct CarlaRadarPublisherImpl {
+    efd::DomainParticipant* _participant { nullptr };
+    efd::Publisher* _publisher { nullptr };
+    efd::Topic* _topic { nullptr };
+    efd::DataWriter* _datawriter { nullptr };
+    efd::TypeSupport _type { new sensor_msgs::msg::PointCloud2PubSubType() };
+    CarlaListener _listener {};
+    sensor_msgs::msg::PointCloud2 _radar {};
+  };
+
+  struct RadarDetectionWithPosition {
+    float x;
+    float y;
+    float z;
+    carla::sensor::data::RadarDetection detection;
+  };
+
+  bool CarlaRadarPublisher::Init() {
+    if (_impl->_type == nullptr) {
+        std::cerr << "Invalid TypeSupport" << std::endl;
+        return false;
+    }
+
+    efd::DomainParticipantQos pqos = efd::PARTICIPANT_QOS_DEFAULT;
+    pqos.name(_name);
+    auto factory = efd::DomainParticipantFactory::get_instance();
+    _impl->_participant = factory->create_participant(0, pqos);
+    if (_impl->_participant == nullptr) {
+        std::cerr << "Failed to create DomainParticipant" << std::endl;
+        return false;
+    }
+    _impl->_type.register_type(_impl->_participant);
+
+    efd::PublisherQos pubqos = efd::PUBLISHER_QOS_DEFAULT;
+    _impl->_publisher = _impl->_participant->create_publisher(pubqos, nullptr);
+    if (_impl->_publisher == nullptr) {
+      std::cerr << "Failed to create Publisher" << std::endl;
+      return false;
+    }
+
+    efd::TopicQos tqos = efd::TOPIC_QOS_DEFAULT;
+    const std::string base { "rt/carla/" };
+    std::string topic_name = base;
+    if (!_parent.empty())
+      topic_name += _parent + "/";
+    topic_name += _name;
+    _impl->_topic = _impl->_participant->create_topic(topic_name, _impl->_type->getName(), tqos);
+    if (_impl->_topic == nullptr) {
+        std::cerr << "Failed to create Topic" << std::endl;
+        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 = (efd::DataWriterListener*)_impl->_listener._impl.get();
+    _impl->_datawriter = _impl->_publisher->create_datawriter(_impl->_topic, wqos, listener);
+    if (_impl->_datawriter == nullptr) {
+        std::cerr << "Failed to create DataWriter" << std::endl;
+        return false;
+    }
+    _frame_id = _name;
+    return true;
+  }
+
+  bool CarlaRadarPublisher::Publish() {
+    eprosima::fastrtps::rtps::InstanceHandle_t instance_handle;
+    eprosima::fastrtps::types::ReturnCode_t rcode = _impl->_datawriter->write(&_impl->_radar, instance_handle);
+    if (rcode == erc::ReturnCodeValue::RETCODE_OK) {
+        return true;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_ERROR) {
+        std::cerr << "RETCODE_ERROR" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_UNSUPPORTED) {
+        std::cerr << "RETCODE_UNSUPPORTED" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_BAD_PARAMETER) {
+        std::cerr << "RETCODE_BAD_PARAMETER" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_PRECONDITION_NOT_MET) {
+        std::cerr << "RETCODE_PRECONDITION_NOT_MET" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_OUT_OF_RESOURCES) {
+        std::cerr << "RETCODE_OUT_OF_RESOURCES" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_NOT_ENABLED) {
+        std::cerr << "RETCODE_NOT_ENABLED" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_IMMUTABLE_POLICY) {
+        std::cerr << "RETCODE_IMMUTABLE_POLICY" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_INCONSISTENT_POLICY) {
+        std::cerr << "RETCODE_INCONSISTENT_POLICY" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_ALREADY_DELETED) {
+        std::cerr << "RETCODE_ALREADY_DELETED" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_TIMEOUT) {
+        std::cerr << "RETCODE_TIMEOUT" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_NO_DATA) {
+        std::cerr << "RETCODE_NO_DATA" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_ILLEGAL_OPERATION) {
+        std::cerr << "RETCODE_ILLEGAL_OPERATION" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_NOT_ALLOWED_BY_SECURITY) {
+        std::cerr << "RETCODE_NOT_ALLOWED_BY_SECURITY" << std::endl;
+        return false;
+    }
+    std::cerr << "UNKNOWN" << std::endl;
+    return false;
+  }
+
+void CarlaRadarPublisher::SetData(int32_t seconds, uint32_t nanoseconds, size_t height, size_t width, size_t elements, const uint8_t* data) {
+
+    std::vector<uint8_t> vector_data;
+    const size_t size = elements * sizeof(RadarDetectionWithPosition);
+    vector_data.resize(size);
+    RadarDetectionWithPosition* radar_data = (RadarDetectionWithPosition*)&vector_data[0];
+    carla::sensor::data::RadarDetection* detection_data = (carla::sensor::data::RadarDetection*)data;
+    for (size_t i = 0; i < elements; ++i, ++radar_data, ++detection_data) {
+      radar_data->x = detection_data->depth * cosf(detection_data->azimuth) * cosf(-detection_data->altitude);
+      radar_data->y = detection_data->depth * sinf(-detection_data->azimuth) * cosf(detection_data->altitude);
+      radar_data->z = detection_data->depth * sinf(detection_data->altitude);
+      radar_data->detection = *detection_data;
+    }
+
+    SetData(seconds, nanoseconds, height, width, elements, std::move(vector_data));
+  }
+
+  void CarlaRadarPublisher::SetData(int32_t seconds, uint32_t nanoseconds, size_t height, size_t width, size_t elements, std::vector<uint8_t>&& data) {
+    builtin_interfaces::msg::Time time;
+    time.sec(seconds);
+    time.nanosec(nanoseconds);
+
+    std_msgs::msg::Header header;
+    header.stamp(std::move(time));
+    header.frame_id(_frame_id);
+
+    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);
+
+    const size_t point_size = sizeof(RadarDetectionWithPosition);
+    _impl->_radar.header(std::move(header));
+    _impl->_radar.width(elements);
+    _impl->_radar.height(height);
+    _impl->_radar.is_bigendian(false);
+    _impl->_radar.fields({descriptor1, descriptor2, descriptor3, descriptor4, descriptor5, descriptor6, descriptor7});
+    _impl->_radar.point_step(point_size);
+    _impl->_radar.row_step(elements * point_size);
+    _impl->_radar.is_dense(false);
+    _impl->_radar.data(std::move(data));
+  }
+
+  CarlaRadarPublisher::CarlaRadarPublisher(const char* ros_name, const char* parent) :
+  _impl(std::make_shared<CarlaRadarPublisherImpl>()) {
+    _name = ros_name;
+    _parent = parent;
+  }
+
+  CarlaRadarPublisher::~CarlaRadarPublisher() {
+      if (!_impl)
+          return;
+
+      if (_impl->_datawriter)
+          _impl->_publisher->delete_datawriter(_impl->_datawriter);
+
+      if (_impl->_publisher)
+          _impl->_participant->delete_publisher(_impl->_publisher);
+
+      if (_impl->_topic)
+          _impl->_participant->delete_topic(_impl->_topic);
+
+      if (_impl->_participant)
+          efd::DomainParticipantFactory::get_instance()->delete_participant(_impl->_participant);
+  }
+
+  CarlaRadarPublisher::CarlaRadarPublisher(const CarlaRadarPublisher& other) {
+    _frame_id = other._frame_id;
+    _name = other._name;
+    _parent = other._parent;
+    _impl = other._impl;
+  }
+
+  CarlaRadarPublisher& CarlaRadarPublisher::operator=(const CarlaRadarPublisher& other) {
+    _frame_id = other._frame_id;
+    _name = other._name;
+    _parent = other._parent;
+    _impl = other._impl;
+
+    return *this;
+  }
+
+  CarlaRadarPublisher::CarlaRadarPublisher(CarlaRadarPublisher&& other) {
+    _frame_id = std::move(other._frame_id);
+    _name = std::move(other._name);
+    _parent = std::move(other._parent);
+    _impl = std::move(other._impl);
+  }
+
+  CarlaRadarPublisher& CarlaRadarPublisher::operator=(CarlaRadarPublisher&& other) {
+    _frame_id = std::move(other._frame_id);
+    _name = std::move(other._name);
+    _parent = std::move(other._parent);
+    _impl = std::move(other._impl);
+
+    return *this;
+  }
+}
+}

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

@@ -0,0 +1,39 @@
+// Copyright (c) 2022 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 <https://opensource.org/licenses/MIT>.
+
+#pragma once
+#define _GLIBCXX_USE_CXX11_ABI 0
+
+#include <memory>
+#include <vector>
+
+#include "CarlaPublisher.h"
+
+namespace carla {
+namespace ros2 {
+
+  struct CarlaRadarPublisherImpl;
+
+  class CarlaRadarPublisher : public CarlaPublisher {
+    public:
+      CarlaRadarPublisher(const char* ros_name = "", const char* parent = "");
+      ~CarlaRadarPublisher();
+      CarlaRadarPublisher(const CarlaRadarPublisher&);
+      CarlaRadarPublisher& operator=(const CarlaRadarPublisher&);
+      CarlaRadarPublisher(CarlaRadarPublisher&&);
+      CarlaRadarPublisher& operator=(CarlaRadarPublisher&&);
+
+      bool Init();
+      bool Publish();
+      void SetData(int32_t seconds, uint32_t nanoseconds, size_t height, size_t width, size_t elements, const uint8_t* data);
+      const char* type() const override { return "radar"; }
+
+    private:
+      void SetData(int32_t seconds, uint32_t nanoseconds, size_t height, size_t width, size_t elements, std::vector<uint8_t>&& data);
+
+    private:
+      std::shared_ptr<CarlaRadarPublisherImpl> _impl;
+  };
+}
+}

LibCarla/source/carla/ros2/publishers/CarlaSSCameraPublisher.cpp

@@ -0,0 +1,414 @@
+#define _GLIBCXX_USE_CXX11_ABI 0
+
+#include "CarlaSSCameraPublisher.h"
+
+#include <string>
+
+#include "carla/ros2/types/ImagePubSubTypes.h"
+#include "carla/ros2/types/CameraInfoPubSubTypes.h"
+#include "carla/ros2/listeners/CarlaListener.h"
+
+#include <fastdds/dds/domain/DomainParticipant.hpp>
+#include <fastdds/dds/publisher/Publisher.hpp>
+#include <fastdds/dds/topic/Topic.hpp>
+#include <fastdds/dds/publisher/DataWriter.hpp>
+#include <fastdds/dds/topic/TypeSupport.hpp>
+
+#include <fastdds/dds/domain/qos/DomainParticipantQos.hpp>
+#include <fastdds/dds/domain/DomainParticipantFactory.hpp>
+#include <fastdds/dds/publisher/qos/PublisherQos.hpp>
+#include <fastdds/dds/topic/qos/TopicQos.hpp>
+
+#include <fastrtps/attributes/ParticipantAttributes.h>
+#include <fastrtps/qos/QosPolicies.h>
+#include <fastdds/dds/publisher/qos/DataWriterQos.hpp>
+#include <fastdds/dds/publisher/DataWriterListener.hpp>
+
+namespace carla {
+namespace ros2 {
+
+  namespace efd = eprosima::fastdds::dds;
+  using erc = eprosima::fastrtps::types::ReturnCode_t;
+
+  struct CarlaSSCameraPublisherImpl {
+    efd::DomainParticipant* _participant { nullptr };
+    efd::Publisher* _publisher { nullptr };
+    efd::Topic* _topic { nullptr };
+    efd::DataWriter* _datawriter { nullptr };
+    efd::TypeSupport _type { new sensor_msgs::msg::ImagePubSubType() };
+    CarlaListener _listener {};
+    sensor_msgs::msg::Image _image {};
+  };
+
+  struct CarlaCameraInfoPublisherImpl {
+    efd::DomainParticipant* _participant { nullptr };
+    efd::Publisher* _publisher { nullptr };
+    efd::Topic* _topic { nullptr };
+    efd::DataWriter* _datawriter { nullptr };
+    efd::TypeSupport _type { new sensor_msgs::msg::CameraInfoPubSubType() };
+    CarlaListener _listener {};
+    bool _init {false};
+    sensor_msgs::msg::CameraInfo _info {};
+  };
+
+  bool CarlaSSCameraPublisher::HasBeenInitialized() const {
+    return _impl_info->_init;
+  }
+
+  void CarlaSSCameraPublisher::InitInfoData(uint32_t x_offset, uint32_t y_offset, uint32_t height, uint32_t width, float fov, bool do_rectify) {
+    _impl_info->_info = std::move(sensor_msgs::msg::CameraInfo(height, width, fov));
+    SetInfoRegionOfInterest(x_offset, y_offset, height, width, do_rectify);
+    _impl_info->_init = true;
+  }
+
+  bool CarlaSSCameraPublisher::Init() {
+    return InitImage() && InitInfo();
+  }
+
+  bool CarlaSSCameraPublisher::InitImage() {
+    if (_impl->_type == nullptr) {
+        std::cerr << "Invalid TypeSupport" << std::endl;
+        return false;
+    }
+    efd::DomainParticipantQos pqos = efd::PARTICIPANT_QOS_DEFAULT;
+    pqos.name(_name);
+    auto factory = efd::DomainParticipantFactory::get_instance();
+    _impl->_participant = factory->create_participant(0, pqos);
+    if (_impl->_participant == nullptr) {
+        std::cerr << "Failed to create DomainParticipant" << std::endl;
+        return false;
+    }
+    _impl->_type.register_type(_impl->_participant);
+
+    efd::PublisherQos pubqos = efd::PUBLISHER_QOS_DEFAULT;
+    _impl->_publisher = _impl->_participant->create_publisher(pubqos, nullptr);
+    if (_impl->_publisher == nullptr) {
+      std::cerr << "Failed to create Publisher" << std::endl;
+      return false;
+    }
+
+    efd::TopicQos tqos = efd::TOPIC_QOS_DEFAULT;
+    const std::string publisher_type {"/image"};
+    const std::string base { "rt/carla/" };
+    std::string topic_name = base;
+    if (!_parent.empty())
+      topic_name += _parent + "/";
+    topic_name += _name;
+    topic_name += publisher_type;
+    _impl->_topic = _impl->_participant->create_topic(topic_name, _impl->_type->getName(), tqos);
+    if (_impl->_topic == nullptr) {
+        std::cerr << "Failed to create Topic" << std::endl;
+        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 = (efd::DataWriterListener*)_impl->_listener._impl.get();
+    _impl->_datawriter = _impl->_publisher->create_datawriter(_impl->_topic, wqos, listener);
+    if (_impl->_datawriter == nullptr) {
+        std::cerr << "Failed to create DataWriter" << std::endl;
+        return false;
+    }
+    _frame_id = _name;
+    return true;
+  }
+
+  bool CarlaSSCameraPublisher::InitInfo() {
+    if (_impl_info->_type == nullptr) {
+        std::cerr << "Invalid TypeSupport" << std::endl;
+        return false;
+    }
+
+    efd::DomainParticipantQos pqos = efd::PARTICIPANT_QOS_DEFAULT;
+    pqos.name(_name);
+    auto factory = efd::DomainParticipantFactory::get_instance();
+    _impl_info->_participant = factory->create_participant(0, pqos);
+    if (_impl_info->_participant == nullptr) {
+        std::cerr << "Failed to create DomainParticipant" << std::endl;
+        return false;
+    }
+    _impl_info->_type.register_type(_impl_info->_participant);
+
+    efd::PublisherQos pubqos = efd::PUBLISHER_QOS_DEFAULT;
+    _impl_info->_publisher = _impl_info->_participant->create_publisher(pubqos, nullptr);
+    if (_impl_info->_publisher == nullptr) {
+      std::cerr << "Failed to create Publisher" << std::endl;
+      return false;
+    }
+
+    efd::TopicQos tqos = efd::TOPIC_QOS_DEFAULT;
+    const std::string publisher_type {"/camera_info"};
+    const std::string base { "rt/carla/" };
+    std::string topic_name = base;
+    if (!_parent.empty())
+      topic_name += _parent + "/";
+    topic_name += _name;
+    topic_name += publisher_type;
+    _impl_info->_topic = _impl_info->_participant->create_topic(topic_name, _impl_info->_type->getName(), tqos);
+    if (_impl_info->_topic == nullptr) {
+        std::cerr << "Failed to create Topic" << std::endl;
+        return false;
+    }
+    efd::DataWriterQos wqos = efd::DATAWRITER_QOS_DEFAULT;
+    efd::DataWriterListener* listener = (efd::DataWriterListener*)_impl_info->_listener._impl.get();
+    _impl_info->_datawriter = _impl_info->_publisher->create_datawriter(_impl_info->_topic, wqos, listener);
+    if (_impl_info->_datawriter == nullptr) {
+        std::cerr << "Failed to create DataWriter" << std::endl;
+        return false;
+    }
+
+    _frame_id = _name;
+    return true;
+  }
+
+  bool CarlaSSCameraPublisher::Publish() {
+    return PublishImage() && PublishInfo();
+  }
+
+  bool CarlaSSCameraPublisher::PublishImage() {
+    eprosima::fastrtps::rtps::InstanceHandle_t instance_handle;
+    eprosima::fastrtps::types::ReturnCode_t rcode = _impl->_datawriter->write(&_impl->_image, instance_handle);
+    if (rcode == erc::ReturnCodeValue::RETCODE_OK) {
+        return true;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_ERROR) {
+        std::cerr << "RETCODE_ERROR" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_UNSUPPORTED) {
+        std::cerr << "RETCODE_UNSUPPORTED" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_BAD_PARAMETER) {
+        std::cerr << "RETCODE_BAD_PARAMETER" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_PRECONDITION_NOT_MET) {
+        std::cerr << "RETCODE_PRECONDITION_NOT_MET" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_OUT_OF_RESOURCES) {
+        std::cerr << "RETCODE_OUT_OF_RESOURCES" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_NOT_ENABLED) {
+        std::cerr << "RETCODE_NOT_ENABLED" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_IMMUTABLE_POLICY) {
+        std::cerr << "RETCODE_IMMUTABLE_POLICY" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_INCONSISTENT_POLICY) {
+        std::cerr << "RETCODE_INCONSISTENT_POLICY" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_ALREADY_DELETED) {
+        std::cerr << "RETCODE_ALREADY_DELETED" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_TIMEOUT) {
+        std::cerr << "RETCODE_TIMEOUT" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_NO_DATA) {
+        std::cerr << "RETCODE_NO_DATA" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_ILLEGAL_OPERATION) {
+        std::cerr << "RETCODE_ILLEGAL_OPERATION" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_NOT_ALLOWED_BY_SECURITY) {
+        std::cerr << "RETCODE_NOT_ALLOWED_BY_SECURITY" << std::endl;
+        return false;
+    }
+    std::cerr << "UNKNOWN" << std::endl;
+    return false;
+  }
+
+  bool CarlaSSCameraPublisher::PublishInfo() {
+    eprosima::fastrtps::rtps::InstanceHandle_t instance_handle;
+    eprosima::fastrtps::types::ReturnCode_t rcode = _impl_info->_datawriter->write(&_impl_info->_info, instance_handle);
+    if (rcode == erc::ReturnCodeValue::RETCODE_OK) {
+        return true;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_ERROR) {
+        std::cerr << "RETCODE_ERROR" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_UNSUPPORTED) {
+        std::cerr << "RETCODE_UNSUPPORTED" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_BAD_PARAMETER) {
+        std::cerr << "RETCODE_BAD_PARAMETER" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_PRECONDITION_NOT_MET) {
+        std::cerr << "RETCODE_PRECONDITION_NOT_MET" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_OUT_OF_RESOURCES) {
+        std::cerr << "RETCODE_OUT_OF_RESOURCES" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_NOT_ENABLED) {
+        std::cerr << "RETCODE_NOT_ENABLED" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_IMMUTABLE_POLICY) {
+        std::cerr << "RETCODE_IMMUTABLE_POLICY" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_INCONSISTENT_POLICY) {
+        std::cerr << "RETCODE_INCONSISTENT_POLICY" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_ALREADY_DELETED) {
+        std::cerr << "RETCODE_ALREADY_DELETED" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_TIMEOUT) {
+        std::cerr << "RETCODE_TIMEOUT" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_NO_DATA) {
+        std::cerr << "RETCODE_NO_DATA" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_ILLEGAL_OPERATION) {
+        std::cerr << "RETCODE_ILLEGAL_OPERATION" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_NOT_ALLOWED_BY_SECURITY) {
+        std::cerr << "RETCODE_NOT_ALLOWED_BY_SECURITY" << std::endl;
+        return false;
+    }
+    std::cerr << "UNKNOWN" << std::endl;
+    return false;
+  }
+
+  void CarlaSSCameraPublisher::SetImageData(int32_t seconds, uint32_t nanoseconds, size_t height, size_t width, const uint8_t* data) {
+    std::vector<uint8_t> vector_data;
+    const size_t size = height * width * 4;
+    vector_data.resize(size);
+    std::memcpy(&vector_data[0], &data[0], size);
+    SetData(seconds, nanoseconds, height, width, std::move(vector_data));
+  }
+
+  void CarlaSSCameraPublisher::SetInfoRegionOfInterest( uint32_t x_offset, uint32_t y_offset, uint32_t height, uint32_t width, bool do_rectify) {
+    sensor_msgs::msg::RegionOfInterest roi;
+    roi.x_offset(x_offset);
+    roi.y_offset(y_offset);
+    roi.height(height);
+    roi.width(width);
+    roi.do_rectify(do_rectify);
+    _impl_info->_info.roi(roi);
+  }
+
+  void CarlaSSCameraPublisher::SetData(int32_t seconds, uint32_t nanoseconds, size_t height, size_t width, std::vector<uint8_t>&& data) {
+    builtin_interfaces::msg::Time time;
+    time.sec(seconds);
+    time.nanosec(nanoseconds);
+
+    std_msgs::msg::Header header;
+    header.stamp(std::move(time));
+    header.frame_id(_frame_id);
+
+    _impl->_image.header(std::move(header));
+    _impl->_image.width(width);
+    _impl->_image.height(height);
+    _impl->_image.encoding("bgra8"); //taken from the list of strings in include/sensor_msgs/image_encodings.h
+    _impl->_image.is_bigendian(0);
+    _impl->_image.step(_impl->_image.width() * sizeof(uint8_t) * 4);
+    _impl->_image.data(std::move(data)); //https://github.com/eProsima/Fast-DDS/issues/2330
+  }
+
+  void CarlaSSCameraPublisher::SetCameraInfoData(int32_t seconds, uint32_t nanoseconds) {
+    builtin_interfaces::msg::Time time;
+    time.sec(seconds);
+    time.nanosec(nanoseconds);
+
+    std_msgs::msg::Header header;
+    header.stamp(std::move(time));
+    header.frame_id(_frame_id);
+    _impl_info->_info.header(header);
+  }
+
+  CarlaSSCameraPublisher::CarlaSSCameraPublisher(const char* ros_name, const char* parent) :
+  _impl(std::make_shared<CarlaSSCameraPublisherImpl>()),
+  _impl_info(std::make_shared<CarlaCameraInfoPublisherImpl>()) {
+    _name = ros_name;
+    _parent = parent;
+  }
+
+  CarlaSSCameraPublisher::~CarlaSSCameraPublisher() {
+      if (!_impl)
+          return;
+
+      if (_impl->_datawriter)
+          _impl->_publisher->delete_datawriter(_impl->_datawriter);
+
+      if (_impl->_publisher)
+          _impl->_participant->delete_publisher(_impl->_publisher);
+
+      if (_impl->_topic)
+          _impl->_participant->delete_topic(_impl->_topic);
+
+      if (_impl->_participant)
+          efd::DomainParticipantFactory::get_instance()->delete_participant(_impl->_participant);
+
+      if (!_impl_info)
+        return;
+
+      if (_impl_info->_datawriter)
+          _impl_info->_publisher->delete_datawriter(_impl_info->_datawriter);
+
+      if (_impl_info->_publisher)
+          _impl_info->_participant->delete_publisher(_impl_info->_publisher);
+
+      if (_impl_info->_topic)
+          _impl_info->_participant->delete_topic(_impl_info->_topic);
+
+      if (_impl_info->_participant)
+          efd::DomainParticipantFactory::get_instance()->delete_participant(_impl_info->_participant);
+  }
+
+  CarlaSSCameraPublisher::CarlaSSCameraPublisher(const CarlaSSCameraPublisher& other) {
+    _frame_id = other._frame_id;
+    _name = other._name;
+    _parent = other._parent;
+    _impl = other._impl;
+    _impl_info = other._impl_info;
+  }
+
+  CarlaSSCameraPublisher& CarlaSSCameraPublisher::operator=(const CarlaSSCameraPublisher& other) {
+    _frame_id = other._frame_id;
+    _name = other._name;
+    _parent = other._parent;
+    _impl = other._impl;
+    _impl_info = other._impl_info;
+
+    return *this;
+  }
+
+  CarlaSSCameraPublisher::CarlaSSCameraPublisher(CarlaSSCameraPublisher&& other) {
+    _frame_id = std::move(other._frame_id);
+    _name = std::move(other._name);
+    _parent = std::move(other._parent);
+    _impl = std::move(other._impl);
+    _impl_info = std::move(other._impl_info);
+  }
+
+  CarlaSSCameraPublisher& CarlaSSCameraPublisher::operator=(CarlaSSCameraPublisher&& other) {
+    _frame_id = std::move(other._frame_id);
+    _name = std::move(other._name);
+    _parent = std::move(other._parent);
+    _impl = std::move(other._impl);
+    _impl_info = std::move(other._impl_info);
+
+    return *this;
+  }
+}
+}

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

@@ -0,0 +1,51 @@
+// Copyright (c) 2022 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 <https://opensource.org/licenses/MIT>.
+
+#pragma once
+#define _GLIBCXX_USE_CXX11_ABI 0
+
+#include <memory>
+#include <vector>
+
+#include "CarlaPublisher.h"
+
+namespace carla {
+namespace ros2 {
+
+  struct CarlaSSCameraPublisherImpl;
+  struct CarlaCameraInfoPublisherImpl;
+
+  class CarlaSSCameraPublisher : public CarlaPublisher {
+    public:
+      CarlaSSCameraPublisher(const char* ros_name = "", const char* parent = "");
+      ~CarlaSSCameraPublisher();
+      CarlaSSCameraPublisher(const CarlaSSCameraPublisher&);
+      CarlaSSCameraPublisher& operator=(const CarlaSSCameraPublisher&);
+      CarlaSSCameraPublisher(CarlaSSCameraPublisher&&);
+      CarlaSSCameraPublisher& operator=(CarlaSSCameraPublisher&&);
+
+      bool Init();
+      void InitInfoData(uint32_t x_offset, uint32_t y_offset, uint32_t height, uint32_t width, float fov, bool do_rectify);
+      bool Publish();
+
+      bool HasBeenInitialized() const;
+      void SetImageData(int32_t seconds, uint32_t nanoseconds, size_t height, size_t width, const uint8_t* data);
+      void SetCameraInfoData(int32_t seconds, uint32_t nanoseconds);
+      const char* type() const override { return "semantic segmentation"; }
+
+    private:
+      bool InitImage();
+      bool InitInfo();
+      bool PublishImage();
+      bool PublishInfo();
+
+      void SetInfoRegionOfInterest( uint32_t x_offset, uint32_t y_offset, uint32_t height, uint32_t width, bool do_rectify);
+      void SetData(int32_t seconds, uint32_t nanoseconds, size_t height, size_t width, std::vector<uint8_t>&& data);
+
+    private:
+      std::shared_ptr<CarlaSSCameraPublisherImpl> _impl;
+      std::shared_ptr<CarlaCameraInfoPublisherImpl> _impl_info;
+  };
+}
+}

LibCarla/source/carla/ros2/publishers/CarlaSemanticLidarPublisher.cpp

@@ -0,0 +1,272 @@
+#define _GLIBCXX_USE_CXX11_ABI 0
+
+#include "CarlaSemanticLidarPublisher.h"
+
+#include <string>
+
+#include "carla/ros2/types/PointCloud2PubSubTypes.h"
+#include "carla/ros2/listeners/CarlaListener.h"
+
+#include <fastdds/dds/domain/DomainParticipant.hpp>
+#include <fastdds/dds/publisher/Publisher.hpp>
+#include <fastdds/dds/topic/Topic.hpp>
+#include <fastdds/dds/publisher/DataWriter.hpp>
+#include <fastdds/dds/topic/TypeSupport.hpp>
+
+#include <fastdds/dds/domain/qos/DomainParticipantQos.hpp>
+#include <fastdds/dds/domain/DomainParticipantFactory.hpp>
+#include <fastdds/dds/publisher/qos/PublisherQos.hpp>
+#include <fastdds/dds/topic/qos/TopicQos.hpp>
+
+#include <fastrtps/attributes/ParticipantAttributes.h>
+#include <fastrtps/qos/QosPolicies.h>
+#include <fastdds/dds/publisher/qos/DataWriterQos.hpp>
+#include <fastdds/dds/publisher/DataWriterListener.hpp>
+
+
+namespace carla {
+namespace ros2 {
+
+  namespace efd = eprosima::fastdds::dds;
+  using erc = eprosima::fastrtps::types::ReturnCode_t;
+
+  struct CarlaSemanticLidarPublisherImpl {
+    efd::DomainParticipant* _participant { nullptr };
+    efd::Publisher* _publisher { nullptr };
+    efd::Topic* _topic { nullptr };
+    efd::DataWriter* _datawriter { nullptr };
+    efd::TypeSupport _type { new sensor_msgs::msg::PointCloud2PubSubType() };
+    CarlaListener _listener {};
+    sensor_msgs::msg::PointCloud2 _lidar {};
+  };
+
+  bool CarlaSemanticLidarPublisher::Init() {
+    if (_impl->_type == nullptr) {
+        std::cerr << "Invalid TypeSupport" << std::endl;
+        return false;
+    }
+    efd::DomainParticipantQos pqos = efd::PARTICIPANT_QOS_DEFAULT;
+    pqos.name(_name);
+    auto factory = efd::DomainParticipantFactory::get_instance();
+    _impl->_participant = factory->create_participant(0, pqos);
+    if (_impl->_participant == nullptr) {
+        std::cerr << "Failed to create DomainParticipant" << std::endl;
+        return false;
+    }
+    _impl->_type.register_type(_impl->_participant);
+
+    efd::PublisherQos pubqos = efd::PUBLISHER_QOS_DEFAULT;
+    _impl->_publisher = _impl->_participant->create_publisher(pubqos, nullptr);
+    if (_impl->_publisher == nullptr) {
+      std::cerr << "Failed to create Publisher" << std::endl;
+      return false;
+    }
+
+    efd::TopicQos tqos = efd::TOPIC_QOS_DEFAULT;
+    const std::string base { "rt/carla/" };
+    std::string topic_name = base;
+    if (!_parent.empty())
+      topic_name += _parent + "/";
+    topic_name += _name;
+    _impl->_topic = _impl->_participant->create_topic(topic_name, _impl->_type->getName(), tqos);
+    if (_impl->_topic == nullptr) {
+        std::cerr << "Failed to create Topic" << std::endl;
+        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 = (efd::DataWriterListener*)_impl->_listener._impl.get();
+    _impl->_datawriter = _impl->_publisher->create_datawriter(_impl->_topic, wqos, listener);
+    if (_impl->_datawriter == nullptr) {
+        std::cerr << "Failed to create DataWriter" << std::endl;
+        return false;
+    }
+    _frame_id = _name;
+    return true;
+  }
+
+  bool CarlaSemanticLidarPublisher::Publish() {
+    eprosima::fastrtps::rtps::InstanceHandle_t instance_handle;
+    eprosima::fastrtps::types::ReturnCode_t rcode = _impl->_datawriter->write(&_impl->_lidar, instance_handle);
+    if (rcode == erc::ReturnCodeValue::RETCODE_OK) {
+        return true;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_ERROR) {
+        std::cerr << "RETCODE_ERROR" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_UNSUPPORTED) {
+        std::cerr << "RETCODE_UNSUPPORTED" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_BAD_PARAMETER) {
+        std::cerr << "RETCODE_BAD_PARAMETER" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_PRECONDITION_NOT_MET) {
+        std::cerr << "RETCODE_PRECONDITION_NOT_MET" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_OUT_OF_RESOURCES) {
+        std::cerr << "RETCODE_OUT_OF_RESOURCES" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_NOT_ENABLED) {
+        std::cerr << "RETCODE_NOT_ENABLED" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_IMMUTABLE_POLICY) {
+        std::cerr << "RETCODE_IMMUTABLE_POLICY" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_INCONSISTENT_POLICY) {
+        std::cerr << "RETCODE_INCONSISTENT_POLICY" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_ALREADY_DELETED) {
+        std::cerr << "RETCODE_ALREADY_DELETED" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_TIMEOUT) {
+        std::cerr << "RETCODE_TIMEOUT" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_NO_DATA) {
+        std::cerr << "RETCODE_NO_DATA" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_ILLEGAL_OPERATION) {
+        std::cerr << "RETCODE_ILLEGAL_OPERATION" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_NOT_ALLOWED_BY_SECURITY) {
+        std::cerr << "RETCODE_NOT_ALLOWED_BY_SECURITY" << std::endl;
+        return false;
+    }
+    std::cerr << "UNKNOWN" << std::endl;
+    return false;
+  }
+
+void CarlaSemanticLidarPublisher::SetData(int32_t seconds, uint32_t nanoseconds, size_t elements, size_t height, size_t width, float* data) {
+    float* it = data;
+    float* end = &data[height * width * elements];
+    for (++it; it < end; it += elements) {
+        *it *= -1.0f;
+    }
+
+    std::vector<uint8_t> vector_data;
+    const size_t size = height * width * sizeof(float) * elements;
+    vector_data.resize(size);
+    std::memcpy(&vector_data[0], &data[0], size);
+    SetData(seconds, nanoseconds, height, width, std::move(vector_data));
+}
+
+void CarlaSemanticLidarPublisher::SetData(int32_t seconds, uint32_t nanoseconds, size_t height, size_t width, std::vector<uint8_t>&& data) {
+    builtin_interfaces::msg::Time time;
+    time.sec(seconds);
+    time.nanosec(nanoseconds);
+
+    std_msgs::msg::Header header;
+    header.stamp(std::move(time));
+    header.frame_id(_frame_id);
+
+    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);
+
+    const size_t point_size = 6 * sizeof(float);
+    _impl->_lidar.header(std::move(header));
+    _impl->_lidar.width(width);
+    _impl->_lidar.height(height);
+    _impl->_lidar.is_bigendian(false);
+    _impl->_lidar.fields({descriptor1, descriptor2, descriptor3, descriptor4, descriptor5, descriptor6});
+    _impl->_lidar.point_step(point_size);
+    _impl->_lidar.row_step(width * point_size);
+    _impl->_lidar.is_dense(false); //True if there are not invalid points
+    _impl->_lidar.data(std::move(data));
+  }
+
+  CarlaSemanticLidarPublisher::CarlaSemanticLidarPublisher(const char* ros_name, const char* parent) :
+  _impl(std::make_shared<CarlaSemanticLidarPublisherImpl>()) {
+    _name = ros_name;
+    _parent = parent;
+  }
+
+  CarlaSemanticLidarPublisher::~CarlaSemanticLidarPublisher() {
+      if (!_impl)
+          return;
+
+      if (_impl->_datawriter)
+          _impl->_publisher->delete_datawriter(_impl->_datawriter);
+
+      if (_impl->_publisher)
+          _impl->_participant->delete_publisher(_impl->_publisher);
+
+      if (_impl->_topic)
+          _impl->_participant->delete_topic(_impl->_topic);
+
+      if (_impl->_participant)
+          efd::DomainParticipantFactory::get_instance()->delete_participant(_impl->_participant);
+  }
+
+  CarlaSemanticLidarPublisher::CarlaSemanticLidarPublisher(const CarlaSemanticLidarPublisher& other) {
+    _frame_id = other._frame_id;
+    _name = other._name;
+    _parent = other._parent;
+    _impl = other._impl;
+  }
+
+  CarlaSemanticLidarPublisher& CarlaSemanticLidarPublisher::operator=(const CarlaSemanticLidarPublisher& other) {
+    _frame_id = other._frame_id;
+    _name = other._name;
+    _parent = other._parent;
+    _impl = other._impl;
+
+    return *this;
+  }
+
+  CarlaSemanticLidarPublisher::CarlaSemanticLidarPublisher(CarlaSemanticLidarPublisher&& other) {
+    _frame_id = std::move(other._frame_id);
+    _name = std::move(other._name);
+    _parent = std::move(other._parent);
+    _impl = std::move(other._impl);
+  }
+
+  CarlaSemanticLidarPublisher& CarlaSemanticLidarPublisher::operator=(CarlaSemanticLidarPublisher&& other) {
+    _frame_id = std::move(other._frame_id);
+    _name = std::move(other._name);
+    _parent = std::move(other._parent);
+    _impl = std::move(other._impl);
+
+    return *this;
+  }
+}
+}

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

@@ -0,0 +1,39 @@
+// Copyright (c) 2022 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 <https://opensource.org/licenses/MIT>.
+
+#pragma once
+#define _GLIBCXX_USE_CXX11_ABI 0
+
+#include <memory>
+#include <vector>
+
+#include "CarlaPublisher.h"
+
+namespace carla {
+namespace ros2 {
+
+  struct CarlaSemanticLidarPublisherImpl;
+
+  class CarlaSemanticLidarPublisher : public CarlaPublisher {
+    public:
+      CarlaSemanticLidarPublisher(const char* ros_name = "", const char* parent = "");
+      ~CarlaSemanticLidarPublisher();
+      CarlaSemanticLidarPublisher(const CarlaSemanticLidarPublisher&);
+      CarlaSemanticLidarPublisher& operator=(const CarlaSemanticLidarPublisher&);
+      CarlaSemanticLidarPublisher(CarlaSemanticLidarPublisher&&);
+      CarlaSemanticLidarPublisher& operator=(CarlaSemanticLidarPublisher&&);
+
+      bool Init();
+      bool Publish();
+      void SetData(int32_t seconds, uint32_t nanoseconds, size_t elements, size_t height, size_t width, float* data);
+      const char* type() const override { return "semantic lidar"; }
+
+    private:
+      void SetData(int32_t seconds, uint32_t nanoseconds, size_t height, size_t width, std::vector<uint8_t>&& data);
+
+    private:
+      std::shared_ptr<CarlaSemanticLidarPublisherImpl> _impl;
+  };
+}
+}

LibCarla/source/carla/ros2/publishers/CarlaSpeedometerSensor.cpp

@@ -0,0 +1,209 @@
+#define _GLIBCXX_USE_CXX11_ABI 0
+
+#include "CarlaSpeedometerSensor.h"
+
+#include <string>
+
+#include "carla/ros2/types/Float32PubSubTypes.h"
+#include "carla/ros2/listeners/CarlaListener.h"
+
+#include <fastdds/dds/domain/DomainParticipant.hpp>
+#include <fastdds/dds/publisher/Publisher.hpp>
+#include <fastdds/dds/topic/Topic.hpp>
+#include <fastdds/dds/publisher/DataWriter.hpp>
+#include <fastdds/dds/topic/TypeSupport.hpp>
+
+#include <fastdds/dds/domain/qos/DomainParticipantQos.hpp>
+#include <fastdds/dds/domain/DomainParticipantFactory.hpp>
+#include <fastdds/dds/publisher/qos/PublisherQos.hpp>
+#include <fastdds/dds/topic/qos/TopicQos.hpp>
+
+#include <fastrtps/attributes/ParticipantAttributes.h>
+#include <fastrtps/qos/QosPolicies.h>
+#include <fastdds/dds/publisher/qos/DataWriterQos.hpp>
+#include <fastdds/dds/publisher/DataWriterListener.hpp>
+
+
+namespace carla {
+namespace ros2 {
+  namespace efd = eprosima::fastdds::dds;
+  using erc = eprosima::fastrtps::types::ReturnCode_t;
+
+  struct CarlaSpeedometerSensorImpl {
+    efd::DomainParticipant* _participant { nullptr };
+    efd::Publisher* _publisher { nullptr };
+    efd::Topic* _topic { nullptr };
+    efd::DataWriter* _datawriter { nullptr };
+    efd::TypeSupport _type { new std_msgs::msg::Float32PubSubType() };
+    CarlaListener _listener {};
+    std_msgs::msg::Float32 _float {};
+  };
+
+  bool CarlaSpeedometerSensor::Init() {
+    if (_impl->_type == nullptr) {
+        std::cerr << "Invalid TypeSupport" << std::endl;
+        return false;
+    }
+    efd::DomainParticipantQos pqos = efd::PARTICIPANT_QOS_DEFAULT;
+    pqos.name(_name);
+    auto factory = efd::DomainParticipantFactory::get_instance();
+    _impl->_participant = factory->create_participant(0, pqos);
+    if (_impl->_participant == nullptr) {
+        std::cerr << "Failed to create DomainParticipant" << std::endl;
+        return false;
+    }
+    _impl->_type.register_type(_impl->_participant);
+
+    efd::PublisherQos pubqos = efd::PUBLISHER_QOS_DEFAULT;
+    _impl->_publisher = _impl->_participant->create_publisher(pubqos, nullptr);
+    if (_impl->_publisher == nullptr) {
+      std::cerr << "Failed to create Publisher" << std::endl;
+      return false;
+    }
+
+    efd::TopicQos tqos = efd::TOPIC_QOS_DEFAULT;
+    const std::string base { "rt/carla/" };
+    std::string topic_name = base;
+    if (!_parent.empty())
+      topic_name += _parent + "/";
+    topic_name += _name;
+    _impl->_topic = _impl->_participant->create_topic(topic_name, _impl->_type->getName(), tqos);
+    if (_impl->_topic == nullptr) {
+        std::cerr << "Failed to create Topic" << std::endl;
+        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 = (efd::DataWriterListener*)_impl->_listener._impl.get();
+    _impl->_datawriter = _impl->_publisher->create_datawriter(_impl->_topic, wqos, listener);
+    if (_impl->_datawriter == nullptr) {
+        std::cerr << "Failed to create DataWriter" << std::endl;
+        return false;
+    }
+    _frame_id = _name;
+    return true;
+  }
+
+  bool CarlaSpeedometerSensor::Publish() {
+    eprosima::fastrtps::rtps::InstanceHandle_t instance_handle;
+    eprosima::fastrtps::types::ReturnCode_t rcode = _impl->_datawriter->write(&_impl->_float, instance_handle);
+    if (rcode == erc::ReturnCodeValue::RETCODE_OK) {
+        return true;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_ERROR) {
+        std::cerr << "RETCODE_ERROR" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_UNSUPPORTED) {
+        std::cerr << "RETCODE_UNSUPPORTED" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_BAD_PARAMETER) {
+        std::cerr << "RETCODE_BAD_PARAMETER" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_PRECONDITION_NOT_MET) {
+        std::cerr << "RETCODE_PRECONDITION_NOT_MET" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_OUT_OF_RESOURCES) {
+        std::cerr << "RETCODE_OUT_OF_RESOURCES" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_NOT_ENABLED) {
+        std::cerr << "RETCODE_NOT_ENABLED" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_IMMUTABLE_POLICY) {
+        std::cerr << "RETCODE_IMMUTABLE_POLICY" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_INCONSISTENT_POLICY) {
+        std::cerr << "RETCODE_INCONSISTENT_POLICY" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_ALREADY_DELETED) {
+        std::cerr << "RETCODE_ALREADY_DELETED" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_TIMEOUT) {
+        std::cerr << "RETCODE_TIMEOUT" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_NO_DATA) {
+        std::cerr << "RETCODE_NO_DATA" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_ILLEGAL_OPERATION) {
+        std::cerr << "RETCODE_ILLEGAL_OPERATION" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_NOT_ALLOWED_BY_SECURITY) {
+        std::cerr << "RETCODE_NOT_ALLOWED_BY_SECURITY" << std::endl;
+        return false;
+    }
+    std::cerr << "UNKNOWN" << std::endl;
+    return false;
+  }
+
+  void CarlaSpeedometerSensor::SetData(float data) {
+    _impl->_float.data(data);
+  }
+
+  CarlaSpeedometerSensor::CarlaSpeedometerSensor(const char* ros_name, const char* parent) :
+  _impl(std::make_shared<CarlaSpeedometerSensorImpl>()) {
+    _name = ros_name;
+    _parent = parent;
+  }
+
+  CarlaSpeedometerSensor::~CarlaSpeedometerSensor() {
+      if (!_impl)
+          return;
+
+      if (_impl->_datawriter)
+          _impl->_publisher->delete_datawriter(_impl->_datawriter);
+
+      if (_impl->_publisher)
+          _impl->_participant->delete_publisher(_impl->_publisher);
+
+      if (_impl->_topic)
+          _impl->_participant->delete_topic(_impl->_topic);
+
+      if (_impl->_participant)
+          efd::DomainParticipantFactory::get_instance()->delete_participant(_impl->_participant);
+  }
+
+  CarlaSpeedometerSensor::CarlaSpeedometerSensor(const CarlaSpeedometerSensor& other) {
+    _frame_id = other._frame_id;
+    _name = other._name;
+    _parent = other._parent;
+    _impl = other._impl;
+  }
+
+  CarlaSpeedometerSensor& CarlaSpeedometerSensor::operator=(const CarlaSpeedometerSensor& other) {
+    _frame_id = other._frame_id;
+    _name = other._name;
+    _parent = other._parent;
+    _impl = other._impl;
+
+    return *this;
+  }
+
+  CarlaSpeedometerSensor::CarlaSpeedometerSensor(CarlaSpeedometerSensor&& other) {
+    _frame_id = std::move(other._frame_id);
+    _name = std::move(other._name);
+    _parent = std::move(other._parent);
+    _impl = std::move(other._impl);
+  }
+
+  CarlaSpeedometerSensor& CarlaSpeedometerSensor::operator=(CarlaSpeedometerSensor&& other) {
+    _frame_id = std::move(other._frame_id);
+    _name = std::move(other._name);
+    _parent = std::move(other._parent);
+    _impl = std::move(other._impl);
+
+    return *this;
+  }
+}
+}

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

@@ -0,0 +1,36 @@
+// Copyright (c) 2022 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 <https://opensource.org/licenses/MIT>.
+
+#pragma once
+#define _GLIBCXX_USE_CXX11_ABI 0
+
+#include <memory>
+#include <vector>
+
+#include "CarlaPublisher.h"
+
+namespace carla {
+namespace ros2 {
+
+  struct CarlaSpeedometerSensorImpl;
+
+  class CarlaSpeedometerSensor : public CarlaPublisher {
+    public:
+      CarlaSpeedometerSensor(const char* ros_name = "", const char* parent = "");
+      ~CarlaSpeedometerSensor();
+      CarlaSpeedometerSensor(const CarlaSpeedometerSensor&);
+      CarlaSpeedometerSensor& operator=(const CarlaSpeedometerSensor&);
+      CarlaSpeedometerSensor(CarlaSpeedometerSensor&&);
+      CarlaSpeedometerSensor& operator=(CarlaSpeedometerSensor&&);
+
+      bool Init();
+      bool Publish();
+      void SetData(float data);
+      const char* type() const override { return "speedometer"; }
+
+    private:
+      std::shared_ptr<CarlaSpeedometerSensorImpl> _impl;
+  };
+}
+}

LibCarla/source/carla/ros2/publishers/CarlaTransformPublisher.cpp

@@ -0,0 +1,259 @@
+#define _GLIBCXX_USE_CXX11_ABI 0
+
+#include "CarlaTransformPublisher.h"
+
+#include <string>
+
+#include "carla/ros2/types/TFMessagePubSubTypes.h"
+#include "carla/ros2/listeners/CarlaListener.h"
+
+#include <fastdds/dds/domain/DomainParticipant.hpp>
+#include <fastdds/dds/publisher/Publisher.hpp>
+#include <fastdds/dds/topic/Topic.hpp>
+#include <fastdds/dds/publisher/DataWriter.hpp>
+#include <fastdds/dds/topic/TypeSupport.hpp>
+
+#include <fastdds/dds/domain/qos/DomainParticipantQos.hpp>
+#include <fastdds/dds/domain/DomainParticipantFactory.hpp>
+#include <fastdds/dds/publisher/qos/PublisherQos.hpp>
+#include <fastdds/dds/topic/qos/TopicQos.hpp>
+
+#include <fastrtps/attributes/ParticipantAttributes.h>
+#include <fastrtps/qos/QosPolicies.h>
+#include <fastdds/dds/publisher/qos/DataWriterQos.hpp>
+#include <fastdds/dds/publisher/DataWriterListener.hpp>
+
+
+namespace carla {
+namespace ros2 {
+  namespace efd = eprosima::fastdds::dds;
+  using erc = eprosima::fastrtps::types::ReturnCode_t;
+
+  struct CarlaTransformPublisherImpl {
+    efd::DomainParticipant* _participant { nullptr };
+    efd::Publisher* _publisher { nullptr };
+    efd::Topic* _topic { nullptr };
+    efd::DataWriter* _datawriter { nullptr };
+    efd::TypeSupport _type { new tf2_msgs::msg::TFMessagePubSubType() };
+    CarlaListener _listener {};
+    tf2_msgs::msg::TFMessage _transform {};
+
+    float last_translation[3] = {0.0f};
+    float last_rotation[3] = {0.0f};
+    geometry_msgs::msg::Vector3 vec_translation;
+    geometry_msgs::msg::Quaternion vec_rotation;
+  };
+
+  bool CarlaTransformPublisher::Init() {
+    if (_impl->_type == nullptr) {
+        std::cerr << "Invalid TypeSupport" << std::endl;
+        return false;
+    }
+
+    efd::DomainParticipantQos pqos = efd::PARTICIPANT_QOS_DEFAULT;
+    pqos.name(_name);
+    auto factory = efd::DomainParticipantFactory::get_instance();
+    _impl->_participant = factory->create_participant(0, pqos);
+    if (_impl->_participant == nullptr) {
+        std::cerr << "Failed to create DomainParticipant" << std::endl;
+        return false;
+    }
+    _impl->_type.register_type(_impl->_participant);
+
+    efd::PublisherQos pubqos = efd::PUBLISHER_QOS_DEFAULT;
+    _impl->_publisher = _impl->_participant->create_publisher(pubqos, nullptr);
+    if (_impl->_publisher == nullptr) {
+      std::cerr << "Failed to create Publisher" << std::endl;
+      return false;
+    }
+
+    efd::TopicQos tqos = efd::TOPIC_QOS_DEFAULT;
+    const std::string topic_name { "rt/tf" };
+    _impl->_topic = _impl->_participant->create_topic(topic_name, _impl->_type->getName(), tqos);
+    if (_impl->_topic == nullptr) {
+        std::cerr << "Failed to create Topic" << std::endl;
+        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 = (efd::DataWriterListener*)_impl->_listener._impl.get();
+    _impl->_datawriter = _impl->_publisher->create_datawriter(_impl->_topic, wqos, listener);
+    if (_impl->_datawriter == nullptr) {
+        std::cerr << "Failed to create DataWriter" << std::endl;
+        return false;
+    }
+    _frame_id = _name;
+    return true;
+  }
+
+  bool CarlaTransformPublisher::Publish() {
+    eprosima::fastrtps::rtps::InstanceHandle_t instance_handle;
+    eprosima::fastrtps::types::ReturnCode_t rcode = _impl->_datawriter->write(&_impl->_transform, instance_handle);
+    if (rcode == erc::ReturnCodeValue::RETCODE_OK) {
+        return true;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_ERROR) {
+        std::cerr << "RETCODE_ERROR" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_UNSUPPORTED) {
+        std::cerr << "RETCODE_UNSUPPORTED" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_BAD_PARAMETER) {
+        std::cerr << "RETCODE_BAD_PARAMETER" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_PRECONDITION_NOT_MET) {
+        std::cerr << "RETCODE_PRECONDITION_NOT_MET" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_OUT_OF_RESOURCES) {
+        std::cerr << "RETCODE_OUT_OF_RESOURCES" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_NOT_ENABLED) {
+        std::cerr << "RETCODE_NOT_ENABLED" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_IMMUTABLE_POLICY) {
+        std::cerr << "RETCODE_IMMUTABLE_POLICY" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_INCONSISTENT_POLICY) {
+        std::cerr << "RETCODE_INCONSISTENT_POLICY" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_ALREADY_DELETED) {
+        std::cerr << "RETCODE_ALREADY_DELETED" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_TIMEOUT) {
+        std::cerr << "RETCODE_TIMEOUT" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_NO_DATA) {
+        std::cerr << "RETCODE_NO_DATA" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_ILLEGAL_OPERATION) {
+        std::cerr << "RETCODE_ILLEGAL_OPERATION" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_NOT_ALLOWED_BY_SECURITY) {
+        std::cerr << "RETCODE_NOT_ALLOWED_BY_SECURITY" << std::endl;
+        return false;
+    }
+    std::cerr << "UNKNOWN" << std::endl;
+    return false;
+  }
+
+  void CarlaTransformPublisher::SetData(int32_t seconds, uint32_t nanoseconds, const float* translation, const float* rotation) {
+
+    int same_translation = std::memcmp(translation, _impl->last_translation, sizeof(float) * 3);
+    int same_rotation = std::memcmp(rotation, _impl->last_rotation, sizeof(float) * 3);
+    if (same_translation != 0 || same_rotation != 0) {
+        std::memcpy(_impl->last_translation, translation, sizeof(float) * 3);
+        std::memcpy(_impl->last_rotation, rotation, sizeof(float) * 3);
+
+        const float tx = *translation++;
+        const float ty = *translation++;
+        const float tz = *translation++;
+
+        const float rx = ((*rotation++) * -1.0f) * (M_PIf32 / 180.0f);
+        const float ry = ((*rotation++) * -1.0f) * (M_PIf32 / 180.0f);
+        const float rz = *rotation++ * (M_PIf32 / 180.0f);
+
+        const float cr = cosf(rz * 0.5f);
+        const float sr = sinf(rz * 0.5f);
+        const float cp = cosf(rx * 0.5f);
+        const float sp = sinf(rx * 0.5f);
+        const float cy = cosf(ry * 0.5f);
+        const float sy = sinf(ry * 0.5f);
+
+        _impl->vec_translation.x(tx);
+        _impl->vec_translation.y(-ty);
+        _impl->vec_translation.z(tz);
+
+        _impl->vec_rotation.w(cr * cp * cy + sr * sp * sy);
+        _impl->vec_rotation.x(sr * cp * cy - cr * sp * sy);
+        _impl->vec_rotation.y(cr * sp * cy + sr * cp * sy);
+        _impl->vec_rotation.z(cr * cp * sy - sr * sp * cy);
+    }
+
+    builtin_interfaces::msg::Time time;
+    time.sec(seconds);
+    time.nanosec(nanoseconds);
+
+    std_msgs::msg::Header header;
+    header.stamp(std::move(time));
+    header.frame_id(_parent);
+
+    geometry_msgs::msg::Transform t;
+    t.rotation(_impl->vec_rotation);
+    t.translation(_impl->vec_translation);
+
+    geometry_msgs::msg::TransformStamped ts;
+    ts.header(std::move(header));
+    ts.transform(std::move(t));
+    ts.child_frame_id(_frame_id);
+    _impl->_transform.transforms({ts});
+  }
+
+  CarlaTransformPublisher::CarlaTransformPublisher(const char* ros_name, const char* parent) :
+  _impl(std::make_shared<CarlaTransformPublisherImpl>()) {
+    _name = ros_name;
+    _parent = parent;
+  }
+
+  CarlaTransformPublisher::~CarlaTransformPublisher() {
+      if (!_impl)
+          return;
+
+      if (_impl->_datawriter)
+          _impl->_publisher->delete_datawriter(_impl->_datawriter);
+
+      if (_impl->_publisher)
+          _impl->_participant->delete_publisher(_impl->_publisher);
+
+      if (_impl->_topic)
+          _impl->_participant->delete_topic(_impl->_topic);
+
+      if (_impl->_participant)
+          efd::DomainParticipantFactory::get_instance()->delete_participant(_impl->_participant);
+  }
+
+  CarlaTransformPublisher::CarlaTransformPublisher(const CarlaTransformPublisher& other) {
+    _frame_id = other._frame_id;
+    _name = other._name;
+    _parent = other._parent;
+    _impl = other._impl;
+  }
+
+  CarlaTransformPublisher& CarlaTransformPublisher::operator=(const CarlaTransformPublisher& other) {
+    _frame_id = other._frame_id;
+    _name = other._name;
+    _parent = other._parent;
+    _impl = other._impl;
+
+    return *this;
+  }
+
+  CarlaTransformPublisher::CarlaTransformPublisher(CarlaTransformPublisher&& other) {
+    _frame_id = std::move(other._frame_id);
+    _name = std::move(other._name);
+    _parent = std::move(other._parent);
+    _impl = std::move(other._impl);
+  }
+
+  CarlaTransformPublisher& CarlaTransformPublisher::operator=(CarlaTransformPublisher&& other) {
+    _frame_id = std::move(other._frame_id);
+    _name = std::move(other._name);
+    _parent = std::move(other._parent);
+    _impl = std::move(other._impl);
+
+    return *this;
+  }
+}
+}

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

@@ -0,0 +1,36 @@
+// Copyright (c) 2022 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 <https://opensource.org/licenses/MIT>.
+
+#pragma once
+#define _GLIBCXX_USE_CXX11_ABI 0
+
+#include <memory>
+#include <vector>
+
+#include "CarlaPublisher.h"
+
+namespace carla {
+namespace ros2 {
+
+  struct CarlaTransformPublisherImpl;
+
+  class CarlaTransformPublisher : public CarlaPublisher {
+    public:
+      CarlaTransformPublisher(const char* ros_name = "", const char* parent = "");
+      ~CarlaTransformPublisher();
+      CarlaTransformPublisher(const CarlaTransformPublisher&);
+      CarlaTransformPublisher& operator=(const CarlaTransformPublisher&);
+      CarlaTransformPublisher(CarlaTransformPublisher&&);
+      CarlaTransformPublisher& operator=(CarlaTransformPublisher&&);
+
+      bool Init();
+      bool Publish();
+      void SetData(int32_t seconds, uint32_t nanoseconds, const float* translation, const float* rotation);
+      const char* type() const override { return "transform"; }
+
+    private:
+      std::shared_ptr<CarlaTransformPublisherImpl> _impl;
+  };
+}
+}

LibCarla/source/carla/ros2/subscribers/CarlaEgoVehicleControlSubscriber.cpp

@@ -0,0 +1,243 @@
+#define _GLIBCXX_USE_CXX11_ABI 0
+
+#include "CarlaEgoVehicleControlSubscriber.h"
+
+#include "carla/ros2/types/CarlaEgoVehicleControl.h"
+#include "carla/ros2/types/CarlaEgoVehicleControlPubSubTypes.h"
+#include "carla/ros2/listeners/CarlaSubscriberListener.h"
+
+#include <fastdds/dds/domain/DomainParticipant.hpp>
+#include <fastdds/dds/subscriber/Subscriber.hpp>
+#include <fastdds/dds/topic/Topic.hpp>
+#include <fastdds/dds/subscriber/DataReader.hpp>
+#include <fastdds/dds/topic/TypeSupport.hpp>
+#include <fastdds/dds/subscriber/SampleInfo.hpp>
+
+#include <fastdds/dds/domain/qos/DomainParticipantQos.hpp>
+#include <fastdds/dds/domain/DomainParticipantFactory.hpp>
+#include <fastdds/dds/subscriber/qos/SubscriberQos.hpp>
+#include <fastdds/dds/topic/qos/TopicQos.hpp>
+
+#include <fastrtps/attributes/ParticipantAttributes.h>
+#include <fastrtps/qos/QosPolicies.h>
+#include <fastdds/dds/subscriber/qos/DataReaderQos.hpp>
+#include <fastdds/dds/subscriber/DataReaderListener.hpp>
+
+
+namespace carla {
+namespace ros2 {
+
+  namespace efd = eprosima::fastdds::dds;
+  using erc = eprosima::fastrtps::types::ReturnCode_t;
+
+  struct CarlaEgoVehicleControlSubscriberImpl {
+    efd::DomainParticipant* _participant { nullptr };
+    efd::Subscriber* _subscriber { nullptr };
+    efd::Topic* _topic { nullptr };
+    efd::DataReader* _datareader { nullptr };
+    efd::TypeSupport _type { new carla_msgs::msg::CarlaEgoVehicleControlPubSubType() };
+    CarlaSubscriberListener _listener {nullptr};
+    carla_msgs::msg::CarlaEgoVehicleControl _event {};
+    VehicleControl _control {};
+    bool _new_message {false};
+    bool _alive {true};
+    void* _vehicle {nullptr};
+  };
+
+  bool CarlaEgoVehicleControlSubscriber::Init() {
+    if (_impl->_type == nullptr) {
+        std::cerr << "Invalid TypeSupport" << std::endl;
+        return false;
+    }
+
+    efd::DomainParticipantQos pqos = efd::PARTICIPANT_QOS_DEFAULT;
+    pqos.name(_name);
+    auto factory = efd::DomainParticipantFactory::get_instance();
+    _impl->_participant = factory->create_participant(0, pqos);
+    if (_impl->_participant == nullptr) {
+        std::cerr << "Failed to create DomainParticipant" << std::endl;
+        return false;
+    }
+    _impl->_type.register_type(_impl->_participant);
+
+    efd::SubscriberQos subqos = efd::SUBSCRIBER_QOS_DEFAULT;
+    _impl->_subscriber = _impl->_participant->create_subscriber(subqos, nullptr);
+    if (_impl->_subscriber == nullptr) {
+      std::cerr << "Failed to create Subscriber" << std::endl;
+      return false;
+    }
+
+    efd::TopicQos tqos = efd::TOPIC_QOS_DEFAULT;
+    const std::string base { "rt/carla/" };
+    const std::string publisher_type {"/vehicle_control_cmd"};
+    std::string topic_name = base;
+    if (!_parent.empty())
+      topic_name += _parent + "/";
+    topic_name += _name;
+    topic_name += publisher_type;
+    _impl->_topic = _impl->_participant->create_topic(topic_name, _impl->_type->getName(), tqos);
+    if (_impl->_topic == nullptr) {
+        std::cerr << "Failed to create Topic" << std::endl;
+        return false;
+    }
+
+    efd::DataReaderQos rqos = efd::DATAREADER_QOS_DEFAULT;
+    efd::DataReaderListener* listener = (efd::DataReaderListener*)_impl->_listener._impl.get();
+    _impl->_datareader = _impl->_subscriber->create_datareader(_impl->_topic, rqos, listener);
+    if (_impl->_datareader == nullptr) {
+        std::cerr << "Failed to create DataReader" << std::endl;
+        return false;
+    }
+    return true;
+  }
+
+  bool CarlaEgoVehicleControlSubscriber::Read() {
+    efd::SampleInfo info;
+    eprosima::fastrtps::types::ReturnCode_t rcode = _impl->_datareader->take_next_sample(&_impl->_event, &info);
+    if (rcode == erc::ReturnCodeValue::RETCODE_OK) {
+        return true;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_ERROR) {
+        std::cerr << "RETCODE_ERROR" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_UNSUPPORTED) {
+        std::cerr << "RETCODE_UNSUPPORTED" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_BAD_PARAMETER) {
+        std::cerr << "RETCODE_BAD_PARAMETER" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_PRECONDITION_NOT_MET) {
+        std::cerr << "RETCODE_PRECONDITION_NOT_MET" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_OUT_OF_RESOURCES) {
+        std::cerr << "RETCODE_OUT_OF_RESOURCES" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_NOT_ENABLED) {
+        std::cerr << "RETCODE_NOT_ENABLED" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_IMMUTABLE_POLICY) {
+        std::cerr << "RETCODE_IMMUTABLE_POLICY" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_INCONSISTENT_POLICY) {
+        std::cerr << "RETCODE_INCONSISTENT_POLICY" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_ALREADY_DELETED) {
+        std::cerr << "RETCODE_ALREADY_DELETED" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_TIMEOUT) {
+        std::cerr << "RETCODE_TIMEOUT" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_NO_DATA) {
+        std::cerr << "RETCODE_NO_DATA" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_ILLEGAL_OPERATION) {
+        std::cerr << "RETCODE_ILLEGAL_OPERATION" << std::endl;
+        return false;
+    }
+    if (rcode == erc::ReturnCodeValue::RETCODE_NOT_ALLOWED_BY_SECURITY) {
+        std::cerr << "RETCODE_NOT_ALLOWED_BY_SECURITY" << std::endl;
+        return false;
+    }
+    std::cerr << "UNKNOWN" << std::endl;
+    return false;
+  }
+
+  void CarlaEgoVehicleControlSubscriber::ForwardMessage(VehicleControl control) {
+    _impl->_control = control;
+    _impl->_new_message = true;
+  }
+
+  void CarlaEgoVehicleControlSubscriber::DestroySubscriber() {
+    _impl->_alive = false;
+  }
+
+  VehicleControl CarlaEgoVehicleControlSubscriber::GetMessage() {
+    _impl->_new_message = false;
+    return _impl->_control;
+  }
+
+  bool CarlaEgoVehicleControlSubscriber::IsAlive() {
+    return _impl->_alive;
+  }
+
+  bool CarlaEgoVehicleControlSubscriber::HasNewMessage() {
+    return _impl->_new_message;
+  }
+
+  void* CarlaEgoVehicleControlSubscriber::GetVehicle() {
+    return _impl->_vehicle;
+  }
+
+  CarlaEgoVehicleControlSubscriber::CarlaEgoVehicleControlSubscriber(void* vehicle, const char* ros_name, const char* parent) :
+  _impl(std::make_shared<CarlaEgoVehicleControlSubscriberImpl>()) {
+    _impl->_listener.SetOwner(this);
+    _impl->_vehicle = vehicle;
+    _name = ros_name;
+    _parent = parent;
+  }
+
+  CarlaEgoVehicleControlSubscriber::~CarlaEgoVehicleControlSubscriber() {
+      if (!_impl)
+          return;
+
+      if (_impl->_datareader)
+          _impl->_subscriber->delete_datareader(_impl->_datareader);
+
+      if (_impl->_subscriber)
+          _impl->_participant->delete_subscriber(_impl->_subscriber);
+
+      if (_impl->_topic)
+          _impl->_participant->delete_topic(_impl->_topic);
+
+      if (_impl->_participant)
+          efd::DomainParticipantFactory::get_instance()->delete_participant(_impl->_participant);
+  }
+
+  CarlaEgoVehicleControlSubscriber::CarlaEgoVehicleControlSubscriber(const CarlaEgoVehicleControlSubscriber& other) {
+    _frame_id = other._frame_id;
+    _name = other._name;
+    _parent = other._parent;
+    _impl = other._impl;
+    _impl->_listener.SetOwner(this);
+  }
+
+  CarlaEgoVehicleControlSubscriber& CarlaEgoVehicleControlSubscriber::operator=(const CarlaEgoVehicleControlSubscriber& other) {
+    _frame_id = other._frame_id;
+    _name = other._name;
+    _parent = other._parent;
+    _impl = other._impl;
+    _impl->_listener.SetOwner(this);
+
+    return *this;
+  }
+
+  CarlaEgoVehicleControlSubscriber::CarlaEgoVehicleControlSubscriber(CarlaEgoVehicleControlSubscriber&& other) {
+    _frame_id = std::move(other._frame_id);
+    _name = std::move(other._name);
+    _parent = std::move(other._parent);
+    _impl = std::move(other._impl);
+    _impl->_listener.SetOwner(this);
+  }
+
+  CarlaEgoVehicleControlSubscriber& CarlaEgoVehicleControlSubscriber::operator=(CarlaEgoVehicleControlSubscriber&& other) {
+    _frame_id = std::move(other._frame_id);
+    _name = std::move(other._name);
+    _parent = std::move(other._parent);
+    _impl = std::move(other._impl);
+    _impl->_listener.SetOwner(this);
+
+    return *this;
+  }
+}
+}

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

@@ -0,0 +1,47 @@
+// Copyright (c) 2022 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 <https://opensource.org/licenses/MIT>.
+
+#pragma once
+#define _GLIBCXX_USE_CXX11_ABI 0
+
+#include <memory>
+#include <vector>
+
+#include "CarlaSubscriber.h"
+#include "carla/ros2/ROS2CallbackData.h"
+
+namespace carla {
+namespace ros2 {
+
+  struct CarlaEgoVehicleControlSubscriberImpl;
+
+  class CarlaEgoVehicleControlSubscriber : public CarlaSubscriber {
+    public:
+      CarlaEgoVehicleControlSubscriber(void* vehicle, const char* ros_name = "", const char* parent = "");
+      ~CarlaEgoVehicleControlSubscriber();
+      CarlaEgoVehicleControlSubscriber(const CarlaEgoVehicleControlSubscriber&);
+      CarlaEgoVehicleControlSubscriber& operator=(const CarlaEgoVehicleControlSubscriber&);
+      CarlaEgoVehicleControlSubscriber(CarlaEgoVehicleControlSubscriber&&);
+      CarlaEgoVehicleControlSubscriber& operator=(CarlaEgoVehicleControlSubscriber&&);
+
+      bool HasNewMessage();
+      bool IsAlive();
+      VehicleControl GetMessage();
+      void* GetVehicle();
+
+      bool Init();
+      bool Read();
+      const char* type() const override { return "Ego vehicle control"; }
+
+      //Do not call, for internal use only
+      void ForwardMessage(VehicleControl control);
+      void DestroySubscriber();
+    private:
+      void SetData(int32_t seconds, uint32_t nanoseconds, uint32_t actor_id, std::vector<float>&& data);
+
+    private:
+      std::shared_ptr<CarlaEgoVehicleControlSubscriberImpl> _impl;
+  };
+}
+}

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

@@ -0,0 +1,35 @@
+// Copyright (c) 2022 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 <https://opensource.org/licenses/MIT>.
+
+#pragma once
+#define _GLIBCXX_USE_CXX11_ABI 0
+
+#include <string>
+
+namespace carla {
+namespace ros2 {
+
+  class CarlaSubscriber {
+    public:
+      const std::string& frame_id() const { return _frame_id; }
+      const std::string& name() const { return _name; }
+      const std::string& parent() const { return _parent; }
+
+      void frame_id(std::string&& frame_id) { _frame_id = std::move(frame_id); }
+      void name(std::string&& name) { _name = std::move(name); }
+      void parent(std::string&& parent) { _parent = std::move(parent); }
+
+      virtual const char* type() const = 0;
+
+    public:
+      CarlaSubscriber() = default;
+      virtual ~CarlaSubscriber() = default;
+
+    protected:
+      std::string _frame_id = "";
+      std::string _name = "";
+      std::string _parent = "";
+  };
+}
+}

LibCarla/source/carla/ros2/types/CameraInfo.cpp

@@ -0,0 +1,628 @@
+// Copyright 2016 Proyectos y Sistemas de Mantenimiento SL (eProsima).
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+/*!
+ * @file CameraInfo.cpp
+ * This source file contains the definition of the described types in the IDL file.
+ *
+ * This file was generated by the tool gen.
+ */
+
+#ifdef _WIN32
+// Remove linker warning LNK4221 on Visual Studio
+namespace {
+char dummy;
+}  // namespace
+#endif  // _WIN32
+
+#include "CameraInfo.h"
+#include <fastcdr/Cdr.h>
+
+#include <fastcdr/exceptions/BadParamException.h>
+using namespace eprosima::fastcdr::exception;
+
+#include <utility>
+#include <cmath>
+
+#define builtin_interfaces_msg_Time_max_cdr_typesize 8ULL;
+#define sensor_msgs_msg_CameraInfo_max_cdr_typesize 3793ULL;
+#define sensor_msgs_msg_RegionOfInterest_max_cdr_typesize 17ULL;
+#define std_msgs_msg_Header_max_cdr_typesize 268ULL;
+#define builtin_interfaces_msg_Time_max_key_cdr_typesize 0ULL;
+#define sensor_msgs_msg_CameraInfo_max_key_cdr_typesize 0ULL;
+#define sensor_msgs_msg_RegionOfInterest_max_key_cdr_typesize 0ULL;
+#define std_msgs_msg_Header_max_key_cdr_typesize 0ULL;
+
+sensor_msgs::msg::CameraInfo::CameraInfo(uint32_t height, uint32_t width, double fov) :
+m_height(height),
+m_width(width)
+{
+    // string m_distortion_model
+    m_distortion_model = "plumb_bob";
+
+    const double cx = static_cast<double>(m_width) / 2.0;
+    const double cy = static_cast<double>(m_height) / 2.0;
+    const double fx = static_cast<double>(m_width) / (2.0 * std::tan(fov) * M_PI / 360.0);
+    const double fy = fx;
+
+    m_d = { 0.0, 0.0, 0.0, 0.0, 0.0 };
+    m_k = {fx, 0.0, cx, 0.0, fy, cy, 0.0, 0.0, 1.0};
+    m_r = { 1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0 };
+    m_p = {fx, 0.0, cx, 0.0, 0.0, fy, cy, 0.0, 0.0, 0.0, 1.0, 0.0};
+
+    m_binning_x = 0;
+    m_binning_y = 0;
+}
+
+sensor_msgs::msg::CameraInfo::~CameraInfo()
+{
+}
+
+sensor_msgs::msg::CameraInfo::CameraInfo(
+        const CameraInfo& x)
+{
+    m_header = x.m_header;
+    m_height = x.m_height;
+    m_width = x.m_width;
+    m_distortion_model = x.m_distortion_model;
+    m_d = x.m_d;
+    m_k = x.m_k;
+    m_r = x.m_r;
+    m_p = x.m_p;
+    m_binning_x = x.m_binning_x;
+    m_binning_y = x.m_binning_y;
+    m_roi = x.m_roi;
+}
+
+sensor_msgs::msg::CameraInfo::CameraInfo(
+        CameraInfo&& x) noexcept
+{
+    m_header = std::move(x.m_header);
+    m_height = x.m_height;
+    m_width = x.m_width;
+    m_distortion_model = std::move(x.m_distortion_model);
+    m_d = std::move(x.m_d);
+    m_k = std::move(x.m_k);
+    m_r = std::move(x.m_r);
+    m_p = std::move(x.m_p);
+    m_binning_x = x.m_binning_x;
+    m_binning_y = x.m_binning_y;
+    m_roi = std::move(x.m_roi);
+}
+
+sensor_msgs::msg::CameraInfo& sensor_msgs::msg::CameraInfo::operator =(
+        const CameraInfo& x)
+{
+    m_header = x.m_header;
+    m_height = x.m_height;
+    m_width = x.m_width;
+    m_distortion_model = x.m_distortion_model;
+    m_d = x.m_d;
+    m_k = x.m_k;
+    m_r = x.m_r;
+    m_p = x.m_p;
+    m_binning_x = x.m_binning_x;
+    m_binning_y = x.m_binning_y;
+    m_roi = x.m_roi;
+
+    return *this;
+}
+
+sensor_msgs::msg::CameraInfo& sensor_msgs::msg::CameraInfo::operator =(
+        CameraInfo&& x) noexcept
+{
+    m_header = std::move(x.m_header);
+    m_height = x.m_height;
+    m_width = x.m_width;
+    m_distortion_model = std::move(x.m_distortion_model);
+    m_d = std::move(x.m_d);
+    m_k = std::move(x.m_k);
+    m_r = std::move(x.m_r);
+    m_p = std::move(x.m_p);
+    m_binning_x = x.m_binning_x;
+    m_binning_y = x.m_binning_y;
+    m_roi = std::move(x.m_roi);
+
+    return *this;
+}
+
+bool sensor_msgs::msg::CameraInfo::operator ==(
+        const CameraInfo& x) const
+{
+    return (m_header == x.m_header && m_height == x.m_height && m_width == x.m_width && m_distortion_model == x.m_distortion_model && m_d == x.m_d && m_k == x.m_k && m_r == x.m_r && m_p == x.m_p && m_binning_x == x.m_binning_x && m_binning_y == x.m_binning_y && m_roi == x.m_roi);
+}
+
+bool sensor_msgs::msg::CameraInfo::operator !=(
+        const CameraInfo& x) const
+{
+    return !(*this == x);
+}
+
+size_t sensor_msgs::msg::CameraInfo::getMaxCdrSerializedSize(
+        size_t current_alignment)
+{
+    static_cast<void>(current_alignment);
+    return sensor_msgs_msg_CameraInfo_max_cdr_typesize;
+}
+
+size_t sensor_msgs::msg::CameraInfo::getCdrSerializedSize(
+        const sensor_msgs::msg::CameraInfo& data,
+        size_t current_alignment)
+{
+    (void)data;
+    size_t initial_alignment = current_alignment;
+    current_alignment += std_msgs::msg::Header::getCdrSerializedSize(data.header(), current_alignment);
+    current_alignment += 4 + eprosima::fastcdr::Cdr::alignment(current_alignment, 4);
+    current_alignment += 4 + eprosima::fastcdr::Cdr::alignment(current_alignment, 4);
+    current_alignment += 4 + eprosima::fastcdr::Cdr::alignment(current_alignment, 4) + data.distortion_model().size() + 1;
+    current_alignment += 4 + eprosima::fastcdr::Cdr::alignment(current_alignment, 4);
+
+    if (data.D().size() > 0)
+    {
+        current_alignment += (data.D().size() * 8) + eprosima::fastcdr::Cdr::alignment(current_alignment, 8);
+    }
+
+    current_alignment += 4 + eprosima::fastcdr::Cdr::alignment(current_alignment, 4);
+
+    if (data.k().size() > 0)
+    {
+        current_alignment += (data.k().size() * 8) + eprosima::fastcdr::Cdr::alignment(current_alignment, 8);
+    }
+
+    current_alignment += 4 + eprosima::fastcdr::Cdr::alignment(current_alignment, 4);
+
+    if (data.r().size() > 0)
+    {
+        current_alignment += (data.r().size() * 8) + eprosima::fastcdr::Cdr::alignment(current_alignment, 8);
+    }
+
+    current_alignment += 4 + eprosima::fastcdr::Cdr::alignment(current_alignment, 4);
+
+    if (data.p().size() > 0)
+    {
+        current_alignment += (data.p().size() * 8) + eprosima::fastcdr::Cdr::alignment(current_alignment, 8);
+    }
+
+    current_alignment += 4 + eprosima::fastcdr::Cdr::alignment(current_alignment, 4);
+    current_alignment += 4 + eprosima::fastcdr::Cdr::alignment(current_alignment, 4);
+    current_alignment += sensor_msgs::msg::RegionOfInterest::getCdrSerializedSize(data.roi(), current_alignment);
+
+    return current_alignment - initial_alignment;
+}
+
+void sensor_msgs::msg::CameraInfo::serialize(
+        eprosima::fastcdr::Cdr& scdr) const
+{
+    scdr << m_header;
+    scdr << m_height;
+    scdr << m_width;
+    scdr << m_distortion_model.c_str();
+    scdr << m_d;
+    scdr << m_k;
+    scdr << m_r;
+    scdr << m_p;
+    scdr << m_binning_x;
+    scdr << m_binning_y;
+    scdr << m_roi;
+}
+
+void sensor_msgs::msg::CameraInfo::deserialize(
+        eprosima::fastcdr::Cdr& dcdr)
+{
+    dcdr >> m_header;
+    dcdr >> m_height;
+    dcdr >> m_width;
+    dcdr >> m_distortion_model;
+    dcdr >> m_d;
+    dcdr >> m_k;
+    dcdr >> m_r;
+    dcdr >> m_p;
+    dcdr >> m_binning_x;
+    dcdr >> m_binning_y;
+    dcdr >> m_roi;
+}
+
+/*!
+ * @brief This function copies the value in member header
+ * @param _header New value to be copied in member header
+ */
+void sensor_msgs::msg::CameraInfo::header(
+        const std_msgs::msg::Header& _header)
+{
+    m_header = _header;
+}
+
+/*!
+ * @brief This function moves the value in member header
+ * @param _header New value to be moved in member header
+ */
+void sensor_msgs::msg::CameraInfo::header(
+        std_msgs::msg::Header&& _header)
+{
+    m_header = std::move(_header);
+}
+
+/*!
+ * @brief This function returns a constant reference to member header
+ * @return Constant reference to member header
+ */
+const std_msgs::msg::Header& sensor_msgs::msg::CameraInfo::header() const
+{
+    return m_header;
+}
+
+/*!
+ * @brief This function returns a reference to member header
+ * @return Reference to member header
+ */
+std_msgs::msg::Header& sensor_msgs::msg::CameraInfo::header()
+{
+    return m_header;
+}
+/*!
+ * @brief This function sets a value in member height
+ * @param _height New value for member height
+ */
+void sensor_msgs::msg::CameraInfo::height(
+        uint32_t _height)
+{
+    m_height = _height;
+}
+
+/*!
+ * @brief This function returns the value of member height
+ * @return Value of member height
+ */
+uint32_t sensor_msgs::msg::CameraInfo::height() const
+{
+    return m_height;
+}
+
+/*!
+ * @brief This function returns a reference to member height
+ * @return Reference to member height
+ */
+uint32_t& sensor_msgs::msg::CameraInfo::height()
+{
+    return m_height;
+}
+
+/*!
+ * @brief This function sets a value in member width
+ * @param _width New value for member width
+ */
+void sensor_msgs::msg::CameraInfo::width(
+        uint32_t _width)
+{
+    m_width = _width;
+}
+
+/*!
+ * @brief This function returns the value of member width
+ * @return Value of member width
+ */
+uint32_t sensor_msgs::msg::CameraInfo::width() const
+{
+    return m_width;
+}
+
+/*!
+ * @brief This function returns a reference to member width
+ * @return Reference to member width
+ */
+uint32_t& sensor_msgs::msg::CameraInfo::width()
+{
+    return m_width;
+}
+
+/*!
+ * @brief This function copies the value in member distortion_model
+ * @param _distortion_model New value to be copied in member distortion_model
+ */
+void sensor_msgs::msg::CameraInfo::distortion_model(
+        const std::string& _distortion_model)
+{
+    m_distortion_model = _distortion_model;
+}
+
+/*!
+ * @brief This function moves the value in member distortion_model
+ * @param _distortion_model New value to be moved in member distortion_model
+ */
+void sensor_msgs::msg::CameraInfo::distortion_model(
+        std::string&& _distortion_model)
+{
+    m_distortion_model = std::move(_distortion_model);
+}
+
+/*!
+ * @brief This function returns a constant reference to member distortion_model
+ * @return Constant reference to member distortion_model
+ */
+const std::string& sensor_msgs::msg::CameraInfo::distortion_model() const
+{
+    return m_distortion_model;
+}
+
+/*!
+ * @brief This function returns a reference to member distortion_model
+ * @return Reference to member distortion_model
+ */
+std::string& sensor_msgs::msg::CameraInfo::distortion_model()
+{
+    return m_distortion_model;
+}
+
+/*!
+ * @brief This function copies the value in member D
+ * @param _D New value to be copied in member D
+ */
+void sensor_msgs::msg::CameraInfo::D(
+        const std::vector<double>& _D)
+{
+    m_d = _D;
+}
+
+/*!
+ * @brief This function moves the value in member D
+ * @param _D New value to be moved in member D
+ */
+void sensor_msgs::msg::CameraInfo::D(
+        std::vector<double>&& _D)
+{
+    m_d = std::move(_D);
+}
+
+/*!
+ * @brief This function returns a constant reference to member D
+ * @return Constant reference to member D
+ */
+const std::vector<double>& sensor_msgs::msg::CameraInfo::D() const
+{
+    return m_d;
+}
+
+/*!
+ * @brief This function returns a reference to member D
+ * @return Reference to member D
+ */
+std::vector<double>& sensor_msgs::msg::CameraInfo::D()
+{
+    return m_d;
+}
+
+/*!
+ * @brief This function copies the value in member k
+ * @param _k New value to be copied in member k
+ */
+void sensor_msgs::msg::CameraInfo::k(
+        const std::array<double, 9>& _k)
+{
+    m_k = _k;
+}
+
+/*!
+ * @brief This function moves the value in member k
+ * @param _k New value to be moved in member k
+ */
+void sensor_msgs::msg::CameraInfo::k(
+        std::array<double, 9>&& _k)
+{
+    m_k = std::move(_k);
+}
+
+/*!
+ * @brief This function returns a constant reference to member k
+ * @return Constant reference to member k
+ */
+const std::array<double, 9>& sensor_msgs::msg::CameraInfo::k() const
+{
+    return m_k;
+}
+
+/*!
+ * @brief This function returns a reference to member k
+ * @return Reference to member k
+ */
+std::array<double, 9>& sensor_msgs::msg::CameraInfo::k()
+{
+    return m_k;
+}
+/*!
+ * @brief This function copies the value in member r
+ * @param _r New value to be copied in member r
+ */
+void sensor_msgs::msg::CameraInfo::r(
+        const std::array<double, 9>& _r)
+{
+    m_r = _r;
+}
+
+/*!
+ * @brief This function moves the value in member r
+ * @param _r New value to be moved in member r
+ */
+void sensor_msgs::msg::CameraInfo::r(
+        std::array<double, 9>&& _r)
+{
+    m_r = std::move(_r);
+}
+
+/*!
+ * @brief This function returns a constant reference to member r
+ * @return Constant reference to member r
+ */
+const std::array<double, 9>& sensor_msgs::msg::CameraInfo::r() const
+{
+    return m_r;
+}
+
+/*!
+ * @brief This function returns a reference to member r
+ * @return Reference to member r
+ */
+std::array<double, 9>& sensor_msgs::msg::CameraInfo::r()
+{
+    return m_r;
+}
+
+/*!
+ * @brief This function copies the value in member p
+ * @param _p New value to be copied in member p
+ */
+void sensor_msgs::msg::CameraInfo::p(
+        const std::array<double, 12>& _p)
+{
+    m_p = _p;
+}
+
+/*!
+ * @brief This function moves the value in member p
+ * @param _p New value to be moved in member p
+ */
+void sensor_msgs::msg::CameraInfo::p(
+        std::array<double, 12>&& _p)
+{
+    m_p = std::move(_p);
+}
+
+/*!
+ * @brief This function returns a constant reference to member p
+ * @return Constant reference to member p
+ */
+const std::array<double, 12>& sensor_msgs::msg::CameraInfo::p() const
+{
+    return m_p;
+}
+
+/*!
+ * @brief This function returns a reference to member p
+ * @return Reference to member p
+ */
+std::array<double, 12>& sensor_msgs::msg::CameraInfo::p()
+{
+    return m_p;
+}
+
+/*!
+ * @brief This function sets a value in member binning_x
+ * @param _binning_x New value for member binning_x
+ */
+void sensor_msgs::msg::CameraInfo::binning_x(
+        uint32_t _binning_x)
+{
+    m_binning_x = _binning_x;
+}
+
+/*!
+ * @brief This function returns the value of member binning_x
+ * @return Value of member binning_x
+ */
+uint32_t sensor_msgs::msg::CameraInfo::binning_x() const
+{
+    return m_binning_x;
+}
+
+/*!
+ * @brief This function returns a reference to member binning_x
+ * @return Reference to member binning_x
+ */
+uint32_t& sensor_msgs::msg::CameraInfo::binning_x()
+{
+    return m_binning_x;
+}
+
+/*!
+ * @brief This function sets a value in member binning_y
+ * @param _binning_y New value for member binning_y
+ */
+void sensor_msgs::msg::CameraInfo::binning_y(
+        uint32_t _binning_y)
+{
+    m_binning_y = _binning_y;
+}
+
+/*!
+ * @brief This function returns the value of member binning_y
+ * @return Value of member binning_y
+ */
+uint32_t sensor_msgs::msg::CameraInfo::binning_y() const
+{
+    return m_binning_y;
+}
+
+/*!
+ * @brief This function returns a reference to member binning_y
+ * @return Reference to member binning_y
+ */
+uint32_t& sensor_msgs::msg::CameraInfo::binning_y()
+{
+    return m_binning_y;
+}
+
+/*!
+ * @brief This function copies the value in member roi
+ * @param _roi New value to be copied in member roi
+ */
+void sensor_msgs::msg::CameraInfo::roi(
+        const sensor_msgs::msg::RegionOfInterest& _roi)
+{
+    m_roi = _roi;
+}
+
+/*!
+ * @brief This function moves the value in member roi
+ * @param _roi New value to be moved in member roi
+ */
+void sensor_msgs::msg::CameraInfo::roi(
+        sensor_msgs::msg::RegionOfInterest&& _roi)
+{
+    m_roi = std::move(_roi);
+}
+
+/*!
+ * @brief This function returns a constant reference to member roi
+ * @return Constant reference to member roi
+ */
+const sensor_msgs::msg::RegionOfInterest& sensor_msgs::msg::CameraInfo::roi() const
+{
+    return m_roi;
+}
+
+/*!
+ * @brief This function returns a reference to member roi
+ * @return Reference to member roi
+ */
+sensor_msgs::msg::RegionOfInterest& sensor_msgs::msg::CameraInfo::roi()
+{
+    return m_roi;
+}
+
+size_t sensor_msgs::msg::CameraInfo::getKeyMaxCdrSerializedSize(
+        size_t current_alignment)
+{
+    static_cast<void>(current_alignment);
+    return sensor_msgs_msg_CameraInfo_max_key_cdr_typesize;
+}
+
+bool sensor_msgs::msg::CameraInfo::isKeyDefined()
+{
+    return false;
+}
+
+void sensor_msgs::msg::CameraInfo::serializeKey(
+        eprosima::fastcdr::Cdr& scdr) const
+{
+    (void) scdr;
+}

LibCarla/source/carla/ros2/types/CameraInfo.h

@@ -0,0 +1,451 @@
+// Copyright 2016 Proyectos y Sistemas de Mantenimiento SL (eProsima).
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+/*!
+ * @file CameraInfo.h
+ * This header file contains the declaration of the described types in the IDL file.
+ *
+ * This file was generated by the tool gen.
+ */
+
+#ifndef _FAST_DDS_GENERATED_SENSOR_MSGS_MSG_CAMERAINFO_H_
+#define _FAST_DDS_GENERATED_SENSOR_MSGS_MSG_CAMERAINFO_H_
+
+#include "RegionOfInterest.h"
+#include "Header.h"
+
+#include <fastrtps/utils/fixed_size_string.hpp>
+
+#include <stdint.h>
+#include <array>
+#include <string>
+#include <vector>
+#include <map>
+#include <bitset>
+
+#if defined(_WIN32)
+#if defined(EPROSIMA_USER_DLL_EXPORT)
+#define eProsima_user_DllExport __declspec( dllexport )
+#else
+#define eProsima_user_DllExport
+#endif  // EPROSIMA_USER_DLL_EXPORT
+#else
+#define eProsima_user_DllExport
+#endif  // _WIN32
+
+#if defined(_WIN32)
+#if defined(EPROSIMA_USER_DLL_EXPORT)
+#if defined(CAMERAINFO_SOURCE)
+#define CAMERAINFO_DllAPI __declspec( dllexport )
+#else
+#define CAMERAINFO_DllAPI __declspec( dllimport )
+#endif // CAMERAINFO_SOURCE
+#else
+#define CAMERAINFO_DllAPI
+#endif  // EPROSIMA_USER_DLL_EXPORT
+#else
+#define CAMERAINFO_DllAPI
+#endif // _WIN32
+
+namespace eprosima {
+namespace fastcdr {
+class Cdr;
+} // namespace fastcdr
+} // namespace eprosima
+
+namespace sensor_msgs {
+    namespace msg {
+        /*!
+         * @brief This class represents the structure CameraInfo defined by the user in the IDL file.
+         * @ingroup CameraInfo
+         */
+        class CameraInfo
+        {
+        public:
+
+            /*!
+             * @brief Default constructor.
+             */
+            eProsima_user_DllExport CameraInfo(uint32_t height = 0, uint32_t width = 0, double fov = 0.0);
+
+            /*!
+             * @brief Default destructor.
+             */
+            eProsima_user_DllExport ~CameraInfo();
+
+            /*!
+             * @brief Copy constructor.
+             * @param x Reference to the object sensor_msgs::msg::CameraInfo that will be copied.
+             */
+            eProsima_user_DllExport CameraInfo(
+                    const CameraInfo& x);
+
+            /*!
+             * @brief Move constructor.
+             * @param x Reference to the object sensor_msgs::msg::CameraInfo that will be copied.
+             */
+            eProsima_user_DllExport CameraInfo(
+                    CameraInfo&& x) noexcept;
+
+            /*!
+             * @brief Copy assignment.
+             * @param x Reference to the object sensor_msgs::msg::CameraInfo that will be copied.
+             */
+            eProsima_user_DllExport CameraInfo& operator =(
+                    const CameraInfo& x);
+
+            /*!
+             * @brief Move assignment.
+             * @param x Reference to the object sensor_msgs::msg::CameraInfo that will be copied.
+             */
+            eProsima_user_DllExport CameraInfo& operator =(
+                    CameraInfo&& x) noexcept;
+
+            /*!
+             * @brief Comparison operator.
+             * @param x sensor_msgs::msg::CameraInfo object to compare.
+             */
+            eProsima_user_DllExport bool operator ==(
+                    const CameraInfo& x) const;
+
+            /*!
+             * @brief Comparison operator.
+             * @param x sensor_msgs::msg::CameraInfo object to compare.
+             */
+            eProsima_user_DllExport bool operator !=(
+                    const CameraInfo& x) const;
+
+            /*!
+             * @brief This function copies the value in member header
+             * @param _header New value to be copied in member header
+             */
+            eProsima_user_DllExport void header(
+                    const std_msgs::msg::Header& _header);
+
+            /*!
+             * @brief This function moves the value in member header
+             * @param _header New value to be moved in member header
+             */
+            eProsima_user_DllExport void header(
+                    std_msgs::msg::Header&& _header);
+
+            /*!
+             * @brief This function returns a constant reference to member header
+             * @return Constant reference to member header
+             */
+            eProsima_user_DllExport const std_msgs::msg::Header& header() const;
+
+            /*!
+             * @brief This function returns a reference to member header
+             * @return Reference to member header
+             */
+            eProsima_user_DllExport std_msgs::msg::Header& header();
+            /*!
+             * @brief This function sets a value in member height
+             * @param _height New value for member height
+             */
+            eProsima_user_DllExport void height(
+                    uint32_t _height);
+
+            /*!
+             * @brief This function returns the value of member height
+             * @return Value of member height
+             */
+            eProsima_user_DllExport uint32_t height() const;
+
+            /*!
+             * @brief This function returns a reference to member height
+             * @return Reference to member height
+             */
+            eProsima_user_DllExport uint32_t& height();
+
+            /*!
+             * @brief This function sets a value in member width
+             * @param _width New value for member width
+             */
+            eProsima_user_DllExport void width(
+                    uint32_t _width);
+
+            /*!
+             * @brief This function returns the value of member width
+             * @return Value of member width
+             */
+            eProsima_user_DllExport uint32_t width() const;
+
+            /*!
+             * @brief This function returns a reference to member width
+             * @return Reference to member width
+             */
+            eProsima_user_DllExport uint32_t& width();
+
+            /*!
+             * @brief This function copies the value in member distortion_model
+             * @param _distortion_model New value to be copied in member distortion_model
+             */
+            eProsima_user_DllExport void distortion_model(
+                    const std::string& _distortion_model);
+
+            /*!
+             * @brief This function moves the value in member distortion_model
+             * @param _distortion_model New value to be moved in member distortion_model
+             */
+            eProsima_user_DllExport void distortion_model(
+                    std::string&& _distortion_model);
+
+            /*!
+             * @brief This function returns a constant reference to member distortion_model
+             * @return Constant reference to member distortion_model
+             */
+            eProsima_user_DllExport const std::string& distortion_model() const;
+
+            /*!
+             * @brief This function returns a reference to member distortion_model
+             * @return Reference to member distortion_model
+             */
+            eProsima_user_DllExport std::string& distortion_model();
+            /*!
+             * @brief This function copies the value in member D
+             * @param _D New value to be copied in member D
+             */
+            eProsima_user_DllExport void D(
+                    const std::vector<double>& _D);
+
+            /*!
+             * @brief This function moves the value in member D
+             * @param _D New value to be moved in member D
+             */
+            eProsima_user_DllExport void D(
+                    std::vector<double>&& _D);
+
+            /*!
+             * @brief This function returns a constant reference to member D
+             * @return Constant reference to member D
+             */
+            eProsima_user_DllExport const std::vector<double>& D() const;
+
+            /*!
+             * @brief This function returns a reference to member D
+             * @return Reference to member D
+             */
+            eProsima_user_DllExport std::vector<double>& D();
+            /*!
+             * @brief This function copies the value in member K
+             * @param _K New value to be copied in member K
+             */
+            eProsima_user_DllExport void k(
+                    const std::array<double, 9>& _k);
+
+        /*!
+        * @brief This function moves the value in member k
+        * @param _k New value to be moved in member k
+        */
+        eProsima_user_DllExport void k(
+                std::array<double, 9>&& _k);
+
+        /*!
+        * @brief This function returns a constant reference to member k
+        * @return Constant reference to member k
+        */
+        eProsima_user_DllExport const std::array<double, 9>& k() const;
+
+        /*!
+        * @brief This function returns a reference to member k
+        * @return Reference to member k
+        */
+        eProsima_user_DllExport std::array<double, 9>& k();
+        /*!
+        * @brief This function copies the value in member r
+        * @param _r New value to be copied in member r
+        */
+        eProsima_user_DllExport void r(
+                const std::array<double, 9>& _r);
+
+        /*!
+        * @brief This function moves the value in member r
+        * @param _r New value to be moved in member r
+        */
+        eProsima_user_DllExport void r(
+                std::array<double, 9>&& _r);
+
+        /*!
+        * @brief This function returns a constant reference to member r
+        * @return Constant reference to member r
+        */
+        eProsima_user_DllExport const std::array<double, 9>& r() const;
+
+        /*!
+        * @brief This function returns a reference to member r
+        * @return Reference to member r
+        */
+        eProsima_user_DllExport std::array<double, 9>& r();
+        /*!
+        * @brief This function copies the value in member p
+        * @param _p New value to be copied in member p
+        */
+        eProsima_user_DllExport void p(
+                const std::array<double, 12>& _p);
+
+        /*!
+        * @brief This function moves the value in member p
+        * @param _p New value to be moved in member p
+        */
+        eProsima_user_DllExport void p(
+                std::array<double, 12>&& _p);
+
+        /*!
+        * @brief This function returns a constant reference to member p
+        * @return Constant reference to member p
+        */
+        eProsima_user_DllExport const std::array<double, 12>& p() const;
+
+        /*!
+        * @brief This function returns a reference to member p
+        * @return Reference to member p
+        */
+        eProsima_user_DllExport std::array<double, 12>& p();
+        /*!
+        * @brief This function sets a value in member binning_x
+        * @param _binning_x New value for member binning_x
+        */
+        eProsima_user_DllExport void binning_x(
+                uint32_t _binning_x);
+
+            /*!
+             * @brief This function returns the value of member binning_x
+             * @return Value of member binning_x
+             */
+            eProsima_user_DllExport uint32_t binning_x() const;
+
+            /*!
+             * @brief This function returns a reference to member binning_x
+             * @return Reference to member binning_x
+             */
+            eProsima_user_DllExport uint32_t& binning_x();
+
+            /*!
+             * @brief This function sets a value in member binning_y
+             * @param _binning_y New value for member binning_y
+             */
+            eProsima_user_DllExport void binning_y(
+                    uint32_t _binning_y);
+
+            /*!
+             * @brief This function returns the value of member binning_y
+             * @return Value of member binning_y
+             */
+            eProsima_user_DllExport uint32_t binning_y() const;
+
+            /*!
+             * @brief This function returns a reference to member binning_y
+             * @return Reference to member binning_y
+             */
+            eProsima_user_DllExport uint32_t& binning_y();
+
+            /*!
+             * @brief This function copies the value in member roi
+             * @param _roi New value to be copied in member roi
+             */
+            eProsima_user_DllExport void roi(
+                    const sensor_msgs::msg::RegionOfInterest& _roi);
+
+            /*!
+             * @brief This function moves the value in member roi
+             * @param _roi New value to be moved in member roi
+             */
+            eProsima_user_DllExport void roi(
+                    sensor_msgs::msg::RegionOfInterest&& _roi);
+
+            /*!
+             * @brief This function returns a constant reference to member roi
+             * @return Constant reference to member roi
+             */
+            eProsima_user_DllExport const sensor_msgs::msg::RegionOfInterest& roi() const;
+
+            /*!
+             * @brief This function returns a reference to member roi
+             * @return Reference to member roi
+             */
+            eProsima_user_DllExport sensor_msgs::msg::RegionOfInterest& roi();
+
+            /*!
+            * @brief This function returns the maximum serialized size of an object
+            * depending on the buffer alignment.
+            * @param current_alignment Buffer alignment.
+            * @return Maximum serialized size.
+            */
+            eProsima_user_DllExport static size_t getMaxCdrSerializedSize(
+                    size_t current_alignment = 0);
+
+            /*!
+             * @brief This function returns the serialized size of a data depending on the buffer alignment.
+             * @param data Data which is calculated its serialized size.
+             * @param current_alignment Buffer alignment.
+             * @return Serialized size.
+             */
+            eProsima_user_DllExport static size_t getCdrSerializedSize(
+                    const sensor_msgs::msg::CameraInfo& data,
+                    size_t current_alignment = 0);
+
+            /*!
+             * @brief This function serializes an object using CDR serialization.
+             * @param cdr CDR serialization object.
+             */
+            eProsima_user_DllExport void serialize(
+                    eprosima::fastcdr::Cdr& cdr) const;
+
+            /*!
+             * @brief This function deserializes an object using CDR serialization.
+             * @param cdr CDR serialization object.
+             */
+            eProsima_user_DllExport void deserialize(
+                    eprosima::fastcdr::Cdr& cdr);
+
+            /*!
+             * @brief This function returns the maximum serialized size of the Key of an object
+             * depending on the buffer alignment.
+             * @param current_alignment Buffer alignment.
+             * @return Maximum serialized size.
+             */
+            eProsima_user_DllExport static size_t getKeyMaxCdrSerializedSize(
+                    size_t current_alignment = 0);
+
+            /*!
+             * @brief This function tells you if the Key has been defined for this type
+             */
+            eProsima_user_DllExport static bool isKeyDefined();
+
+            /*!
+             * @brief This function serializes the key members of an object using CDR serialization.
+             * @param cdr CDR serialization object.
+             */
+            eProsima_user_DllExport void serializeKey(
+                    eprosima::fastcdr::Cdr& cdr) const;
+
+        private:
+        std_msgs::msg::Header m_header;
+        uint32_t m_height;
+        uint32_t m_width;
+        std::string m_distortion_model;
+        std::vector<double> m_d;
+        std::array<double, 9> m_k;
+        std::array<double, 9> m_r;
+        std::array<double, 12> m_p;
+        uint32_t m_binning_x;
+        uint32_t m_binning_y;
+        sensor_msgs::msg::RegionOfInterest m_roi;
+        };
+    } // namespace msg
+} // namespace sensor_msgs
+
+#endif // _FAST_DDS_GENERATED_SENSOR_MSGS_MSG_CAMERAINFO_H_

LibCarla/source/carla/ros2/types/CameraInfoPubSubTypes.cpp

@@ -0,0 +1,172 @@
+// Copyright 2016 Proyectos y Sistemas de Mantenimiento SL (eProsima).
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+/*!
+ * @file CameraInfoPubSubTypes.cpp
+ * This header file contains the implementation of the serialization functions.
+ *
+ * This file was generated by the tool fastcdrgen.
+ */
+
+#include <fastcdr/FastBuffer.h>
+#include <fastcdr/Cdr.h>
+
+#include "CameraInfoPubSubTypes.h"
+
+using SerializedPayload_t = eprosima::fastrtps::rtps::SerializedPayload_t;
+using InstanceHandle_t = eprosima::fastrtps::rtps::InstanceHandle_t;
+
+namespace sensor_msgs {
+    namespace msg {
+        CameraInfoPubSubType::CameraInfoPubSubType()
+        {
+            setName("sensor_msgs::msg::dds_::CameraInfo_");
+            auto type_size = CameraInfo::getMaxCdrSerializedSize();
+            type_size += eprosima::fastcdr::Cdr::alignment(type_size, 4); /* possible submessage alignment */
+            m_typeSize = static_cast<uint32_t>(type_size) + 4; /*encapsulation*/
+            m_isGetKeyDefined = CameraInfo::isKeyDefined();
+            size_t keyLength = CameraInfo::getKeyMaxCdrSerializedSize() > 16 ?
+                    CameraInfo::getKeyMaxCdrSerializedSize() : 16;
+            m_keyBuffer = reinterpret_cast<unsigned char*>(malloc(keyLength));
+            memset(m_keyBuffer, 0, keyLength);
+        }
+
+        CameraInfoPubSubType::~CameraInfoPubSubType()
+        {
+            if (m_keyBuffer != nullptr)
+            {
+                free(m_keyBuffer);
+            }
+        }
+
+        bool CameraInfoPubSubType::serialize(
+                void* data,
+                SerializedPayload_t* payload)
+        {
+            CameraInfo* p_type = static_cast<CameraInfo*>(data);
+
+            // Object that manages the raw buffer.
+            eprosima::fastcdr::FastBuffer fastbuffer(reinterpret_cast<char*>(payload->data), payload->max_size);
+            // Object that serializes the data.
+            eprosima::fastcdr::Cdr ser(fastbuffer, eprosima::fastcdr::Cdr::DEFAULT_ENDIAN, eprosima::fastcdr::Cdr::DDS_CDR);
+            payload->encapsulation = ser.endianness() == eprosima::fastcdr::Cdr::BIG_ENDIANNESS ? CDR_BE : CDR_LE;
+
+            try
+            {
+                // Serialize encapsulation
+                ser.serialize_encapsulation();
+                // Serialize the object.
+                p_type->serialize(ser);
+            }
+            catch (eprosima::fastcdr::exception::Exception& /*exception*/)
+            {
+                return false;
+            }
+
+            // Get the serialized length
+            payload->length = static_cast<uint32_t>(ser.getSerializedDataLength());
+            return true;
+        }
+
+        bool CameraInfoPubSubType::deserialize(
+                SerializedPayload_t* payload,
+                void* data)
+        {
+            try
+            {
+                // Convert DATA to pointer of your type
+                CameraInfo* p_type = static_cast<CameraInfo*>(data);
+
+                // Object that manages the raw buffer.
+                eprosima::fastcdr::FastBuffer fastbuffer(reinterpret_cast<char*>(payload->data), payload->length);
+
+                // Object that deserializes the data.
+                eprosima::fastcdr::Cdr deser(fastbuffer, eprosima::fastcdr::Cdr::DEFAULT_ENDIAN, eprosima::fastcdr::Cdr::DDS_CDR);
+
+                // Deserialize encapsulation.
+                deser.read_encapsulation();
+                payload->encapsulation = deser.endianness() == eprosima::fastcdr::Cdr::BIG_ENDIANNESS ? CDR_BE : CDR_LE;
+
+                // Deserialize the object.
+                p_type->deserialize(deser);
+            }
+            catch (eprosima::fastcdr::exception::Exception& /*exception*/)
+            {
+                return false;
+            }
+
+            return true;
+        }
+
+        std::function<uint32_t()> CameraInfoPubSubType::getSerializedSizeProvider(
+                void* data)
+        {
+            return [data]() -> uint32_t
+                   {
+                       return static_cast<uint32_t>(type::getCdrSerializedSize(*static_cast<CameraInfo*>(data))) +
+                              4u /*encapsulation*/;
+                   };
+        }
+
+        void* CameraInfoPubSubType::createData()
+        {
+            return reinterpret_cast<void*>(new CameraInfo());
+        }
+
+        void CameraInfoPubSubType::deleteData(
+                void* data)
+        {
+            delete(reinterpret_cast<CameraInfo*>(data));
+        }
+
+        bool CameraInfoPubSubType::getKey(
+                void* data,
+                InstanceHandle_t* handle,
+                bool force_md5)
+        {
+            if (!m_isGetKeyDefined)
+            {
+                return false;
+            }
+
+            CameraInfo* p_type = static_cast<CameraInfo*>(data);
+
+            // Object that manages the raw buffer.
+            eprosima::fastcdr::FastBuffer fastbuffer(reinterpret_cast<char*>(m_keyBuffer),
+                    CameraInfo::getKeyMaxCdrSerializedSize());
+
+            // Object that serializes the data.
+            eprosima::fastcdr::Cdr ser(fastbuffer, eprosima::fastcdr::Cdr::BIG_ENDIANNESS);
+            p_type->serializeKey(ser);
+            if (force_md5 || CameraInfo::getKeyMaxCdrSerializedSize() > 16)
+            {
+                m_md5.init();
+                m_md5.update(m_keyBuffer, static_cast<unsigned int>(ser.getSerializedDataLength()));
+                m_md5.finalize();
+                for (uint8_t i = 0; i < 16; ++i)
+                {
+                    handle->value[i] = m_md5.digest[i];
+                }
+            }
+            else
+            {
+                for (uint8_t i = 0; i < 16; ++i)
+                {
+                    handle->value[i] = m_keyBuffer[i];
+                }
+            }
+            return true;
+        }
+    } //End of namespace msg
+} //End of namespace sensor_msgs

LibCarla/source/carla/ros2/types/CameraInfoPubSubTypes.h

@@ -0,0 +1,108 @@
+// Copyright 2016 Proyectos y Sistemas de Mantenimiento SL (eProsima).
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+/*!
+ * @file CameraInfoPubSubTypes.h
+ * This header file contains the declaration of the serialization functions.
+ *
+ * This file was generated by the tool fastcdrgen.
+ */
+
+#ifndef _FAST_DDS_GENERATED_SENSOR_MSGS_MSG_CAMERAINFO_PUBSUBTYPES_H_
+#define _FAST_DDS_GENERATED_SENSOR_MSGS_MSG_CAMERAINFO_PUBSUBTYPES_H_
+
+#include <fastdds/dds/topic/TopicDataType.hpp>
+#include <fastrtps/utils/md5.h>
+
+#include "CameraInfo.h"
+
+#include "RegionOfInterestPubSubTypes.h"
+#include "HeaderPubSubTypes.h"
+
+#if !defined(GEN_API_VER) || (GEN_API_VER != 1)
+#error \
+    Generated CameraInfo is not compatible with current installed Fast DDS. Please, regenerate it with fastddsgen.
+#endif  // GEN_API_VER
+
+namespace sensor_msgs
+{
+    namespace msg
+    {
+        /*!
+         * @brief This class represents the TopicDataType of the type CameraInfo defined by the user in the IDL file.
+         * @ingroup CameraInfo
+         */
+        class CameraInfoPubSubType : public eprosima::fastdds::dds::TopicDataType
+        {
+        public:
+
+            typedef CameraInfo type;
+
+            eProsima_user_DllExport CameraInfoPubSubType();
+
+            eProsima_user_DllExport virtual ~CameraInfoPubSubType() override;
+
+            eProsima_user_DllExport virtual bool serialize(
+                    void* data,
+                    eprosima::fastrtps::rtps::SerializedPayload_t* payload) override;
+
+            eProsima_user_DllExport virtual bool deserialize(
+                    eprosima::fastrtps::rtps::SerializedPayload_t* payload,
+                    void* data) override;
+
+            eProsima_user_DllExport virtual std::function<uint32_t()> getSerializedSizeProvider(
+                    void* data) override;
+
+            eProsima_user_DllExport virtual bool getKey(
+                    void* data,
+                    eprosima::fastrtps::rtps::InstanceHandle_t* ihandle,
+                    bool force_md5 = false) override;
+
+            eProsima_user_DllExport virtual void* createData() override;
+
+            eProsima_user_DllExport virtual void deleteData(
+                    void* data) override;
+
+        #ifdef TOPIC_DATA_TYPE_API_HAS_IS_BOUNDED
+            eProsima_user_DllExport inline bool is_bounded() const override
+            {
+                return false;
+            }
+
+        #endif  // TOPIC_DATA_TYPE_API_HAS_IS_BOUNDED
+
+        #ifdef TOPIC_DATA_TYPE_API_HAS_IS_PLAIN
+            eProsima_user_DllExport inline bool is_plain() const override
+            {
+                return false;
+            }
+
+        #endif  // TOPIC_DATA_TYPE_API_HAS_IS_PLAIN
+
+        #ifdef TOPIC_DATA_TYPE_API_HAS_CONSTRUCT_SAMPLE
+            eProsima_user_DllExport inline bool construct_sample(
+                    void* memory) const override
+            {
+                (void)memory;
+                return false;
+            }
+
+        #endif  // TOPIC_DATA_TYPE_API_HAS_CONSTRUCT_SAMPLE
+            MD5 m_md5;
+            unsigned char* m_keyBuffer;
+        };
+    }
+}
+
+#endif // _FAST_DDS_GENERATED_SENSOR_MSGS_MSG_CAMERAINFO_PUBSUBTYPES_H_

LibCarla/source/carla/ros2/types/CarlaCollisionEvent.cpp

@@ -0,0 +1,261 @@
+// Copyright 2016 Proyectos y Sistemas de Mantenimiento SL (eProsima).
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+/*!
+ * @file CarlaCollisionEvent.cpp
+ * This source file contains the definition of the described types in the IDL file.
+ *
+ * This file was generated by the tool gen.
+ */
+
+#ifdef _WIN32
+// Remove linker warning LNK4221 on Visual Studio
+namespace {
+char dummy;
+}  // namespace
+#endif  // _WIN32
+
+#include "CarlaCollisionEvent.h"
+#include <fastcdr/Cdr.h>
+
+#include <fastcdr/exceptions/BadParamException.h>
+using namespace eprosima::fastcdr::exception;
+
+#include <utility>
+
+#define carla_msgs_msg_geometry_msgs_msg_Vector3_max_cdr_typesize 24ULL;
+#define carla_msgs_msg_std_msgs_msg_Header_max_cdr_typesize 268ULL;
+#define carla_msgs_msg_CollisionEvent_max_cdr_typesize 296ULL;
+#define carla_msgs_msg_std_msgs_msg_Time_max_cdr_typesize 8ULL;
+#define carla_msgs_msg_geometry_msgs_msg_Vector3_max_key_cdr_typesize 0ULL;
+#define carla_msgs_msg_std_msgs_msg_Header_max_key_cdr_typesize 0ULL;
+#define carla_msgs_msg_CollisionEvent_max_key_cdr_typesize 0ULL;
+#define carla_msgs_msg_std_msgs_msg_Time_max_key_cdr_typesize 0ULL;
+
+carla_msgs::msg::CollisionEvent::CollisionEvent()
+{
+    // std_msgs::msg::Header m_header
+    // unsigned long m_other_actor_id
+    m_other_actor_id = 0;
+    // geometry_msgs::msg::Vector3 m_normal_impulse
+}
+
+carla_msgs::msg::CollisionEvent::~CollisionEvent()
+{
+}
+
+carla_msgs::msg::CollisionEvent::CollisionEvent(
+        const CollisionEvent& x)
+{
+    m_header = x.m_header;
+    m_other_actor_id = x.m_other_actor_id;
+    m_normal_impulse = x.m_normal_impulse;
+}
+
+carla_msgs::msg::CollisionEvent::CollisionEvent(
+        CollisionEvent&& x) noexcept
+{
+    m_header = std::move(x.m_header);
+    m_other_actor_id = x.m_other_actor_id;
+    m_normal_impulse = std::move(x.m_normal_impulse);
+}
+
+carla_msgs::msg::CollisionEvent& carla_msgs::msg::CollisionEvent::operator =(
+        const CollisionEvent& x)
+{
+    m_header = x.m_header;
+    m_other_actor_id = x.m_other_actor_id;
+    m_normal_impulse = x.m_normal_impulse;
+
+    return *this;
+}
+
+carla_msgs::msg::CollisionEvent& carla_msgs::msg::CollisionEvent::operator =(
+        CollisionEvent&& x) noexcept
+{
+    m_header = std::move(x.m_header);
+    m_other_actor_id = x.m_other_actor_id;
+    m_normal_impulse = std::move(x.m_normal_impulse);
+
+    return *this;
+}
+
+bool carla_msgs::msg::CollisionEvent::operator ==(
+        const CollisionEvent& x) const
+{
+    return (m_header == x.m_header && m_other_actor_id == x.m_other_actor_id && m_normal_impulse == x.m_normal_impulse);
+}
+
+bool carla_msgs::msg::CollisionEvent::operator !=(
+        const CollisionEvent& x) const
+{
+    return !(*this == x);
+}
+
+size_t carla_msgs::msg::CollisionEvent::getMaxCdrSerializedSize(
+        size_t current_alignment)
+{
+    static_cast<void>(current_alignment);
+    return carla_msgs_msg_CollisionEvent_max_cdr_typesize;
+}
+
+size_t carla_msgs::msg::CollisionEvent::getCdrSerializedSize(
+        const carla_msgs::msg::CollisionEvent& data,
+        size_t current_alignment)
+{
+    (void)data;
+    size_t initial_alignment = current_alignment;
+    current_alignment += std_msgs::msg::Header::getCdrSerializedSize(data.header(), current_alignment);
+    current_alignment += 4 + eprosima::fastcdr::Cdr::alignment(current_alignment, 4);
+    current_alignment += geometry_msgs::msg::Vector3::getCdrSerializedSize(data.normal_impulse(), current_alignment);
+    return current_alignment - initial_alignment;
+}
+
+void carla_msgs::msg::CollisionEvent::serialize(
+        eprosima::fastcdr::Cdr& scdr) const
+{
+    scdr << m_header;
+    scdr << m_other_actor_id;
+    scdr << m_normal_impulse;
+}
+
+void carla_msgs::msg::CollisionEvent::deserialize(
+        eprosima::fastcdr::Cdr& dcdr)
+{
+    dcdr >> m_header;
+    dcdr >> m_other_actor_id;
+    dcdr >> m_normal_impulse;
+}
+
+/*!
+ * @brief This function copies the value in member header
+ * @param _header New value to be copied in member header
+ */
+void carla_msgs::msg::CollisionEvent::header(
+        const std_msgs::msg::Header& _header)
+{
+    m_header = _header;
+}
+
+/*!
+ * @brief This function moves the value in member header
+ * @param _header New value to be moved in member header
+ */
+void carla_msgs::msg::CollisionEvent::header(
+        std_msgs::msg::Header&& _header)
+{
+    m_header = std::move(_header);
+}
+
+/*!
+ * @brief This function returns a constant reference to member header
+ * @return Constant reference to member header
+ */
+const std_msgs::msg::Header& carla_msgs::msg::CollisionEvent::header() const
+{
+    return m_header;
+}
+
+/*!
+ * @brief This function returns a reference to member header
+ * @return Reference to member header
+ */
+std_msgs::msg::Header& carla_msgs::msg::CollisionEvent::header()
+{
+    return m_header;
+}
+
+/*!
+ * @brief This function sets a value in member other_actor_id
+ * @param _other_actor_id New value for member other_actor_id
+ */
+void carla_msgs::msg::CollisionEvent::other_actor_id(
+        uint32_t _other_actor_id)
+{
+    m_other_actor_id = _other_actor_id;
+}
+
+/*!
+ * @brief This function returns the value of member other_actor_id
+ * @return Value of member other_actor_id
+ */
+uint32_t carla_msgs::msg::CollisionEvent::other_actor_id() const
+{
+    return m_other_actor_id;
+}
+
+/*!
+ * @brief This function returns a reference to member other_actor_id
+ * @return Reference to member other_actor_id
+ */
+uint32_t& carla_msgs::msg::CollisionEvent::other_actor_id()
+{
+    return m_other_actor_id;
+}
+
+/*!
+ * @brief This function copies the value in member normal_impulse
+ * @param _normal_impulse New value to be copied in member normal_impulse
+ */
+void carla_msgs::msg::CollisionEvent::normal_impulse(
+        const geometry_msgs::msg::Vector3& _normal_impulse)
+{
+    m_normal_impulse = _normal_impulse;
+}
+
+/*!
+ * @brief This function moves the value in member normal_impulse
+ * @param _normal_impulse New value to be moved in member normal_impulse
+ */
+void carla_msgs::msg::CollisionEvent::normal_impulse(
+        geometry_msgs::msg::Vector3&& _normal_impulse)
+{
+    m_normal_impulse = std::move(_normal_impulse);
+}
+
+/*!
+ * @brief This function returns a constant reference to member normal_impulse
+ * @return Constant reference to member normal_impulse
+ */
+const geometry_msgs::msg::Vector3& carla_msgs::msg::CollisionEvent::normal_impulse() const
+{
+    return m_normal_impulse;
+}
+
+/*!
+ * @brief This function returns a reference to member normal_impulse
+ * @return Reference to member normal_impulse
+ */
+geometry_msgs::msg::Vector3& carla_msgs::msg::CollisionEvent::normal_impulse()
+{
+    return m_normal_impulse;
+}
+
+size_t carla_msgs::msg::CollisionEvent::getKeyMaxCdrSerializedSize(
+        size_t current_alignment)
+{
+    static_cast<void>(current_alignment);
+    return carla_msgs_msg_CollisionEvent_max_key_cdr_typesize;
+}
+
+bool carla_msgs::msg::CollisionEvent::isKeyDefined()
+{
+    return false;
+}
+
+void carla_msgs::msg::CollisionEvent::serializeKey(
+        eprosima::fastcdr::Cdr& scdr) const
+{
+    (void) scdr;
+}

LibCarla/source/carla/ros2/types/CarlaCollisionEvent.h

@@ -0,0 +1,262 @@
+// Copyright 2016 Proyectos y Sistemas de Mantenimiento SL (eProsima).
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+/*!
+ * @file CarlaCollisionEvent.h
+ * This header file contains the declaration of the described types in the IDL file.
+ *
+ * This file was generated by the tool gen.
+ */
+
+#ifndef _FAST_DDS_GENERATED_CARLA_MSGS_MSG_CARLACOLLISIONEVENT_H_
+#define _FAST_DDS_GENERATED_CARLA_MSGS_MSG_CARLACOLLISIONEVENT_H_
+
+#include "Vector3.h"
+#include "Header.h"
+
+#include <fastrtps/utils/fixed_size_string.hpp>
+
+#include <stdint.h>
+#include <array>
+#include <string>
+#include <vector>
+#include <map>
+#include <bitset>
+
+#if defined(_WIN32)
+#if defined(EPROSIMA_USER_DLL_EXPORT)
+#define eProsima_user_DllExport __declspec( dllexport )
+#else
+#define eProsima_user_DllExport
+#endif  // EPROSIMA_USER_DLL_EXPORT
+#else
+#define eProsima_user_DllExport
+#endif  // _WIN32
+
+#if defined(_WIN32)
+#if defined(EPROSIMA_USER_DLL_EXPORT)
+#if defined(CarlaCollisionEvent_SOURCE)
+#define CarlaCollisionEvent_DllAPI __declspec( dllexport )
+#else
+#define CarlaCollisionEvent_DllAPI __declspec( dllimport )
+#endif // CarlaCollisionEvent_SOURCE
+#else
+#define CarlaCollisionEvent_DllAPI
+#endif  // EPROSIMA_USER_DLL_EXPORT
+#else
+#define CarlaCollisionEvent_DllAPI
+#endif // _WIN32
+
+namespace eprosima {
+namespace fastcdr {
+class Cdr;
+} // namespace fastcdr
+} // namespace eprosima
+
+
+namespace carla_msgs {
+    namespace msg {
+        /*!
+         * @brief This class represents the structure CollisionEvent defined by the user in the IDL file.
+         * @ingroup CARLACOLLISIONEVENT
+         */
+        class CollisionEvent
+        {
+        public:
+
+            /*!
+             * @brief Default constructor.
+             */
+            eProsima_user_DllExport CollisionEvent();
+
+            /*!
+             * @brief Default destructor.
+             */
+            eProsima_user_DllExport ~CollisionEvent();
+
+            /*!
+             * @brief Copy constructor.
+             * @param x Reference to the object carla_msgs::msg::CollisionEvent that will be copied.
+             */
+            eProsima_user_DllExport CollisionEvent(
+                    const CollisionEvent& x);
+
+            /*!
+             * @brief Move constructor.
+             * @param x Reference to the object carla_msgs::msg::CollisionEvent that will be copied.
+             */
+            eProsima_user_DllExport CollisionEvent(
+                    CollisionEvent&& x) noexcept;
+
+            /*!
+             * @brief Copy assignment.
+             * @param x Reference to the object carla_msgs::msg::CollisionEvent that will be copied.
+             */
+            eProsima_user_DllExport CollisionEvent& operator =(
+                    const CollisionEvent& x);
+
+            /*!
+             * @brief Move assignment.
+             * @param x Reference to the object carla_msgs::msg::CollisionEvent that will be copied.
+             */
+            eProsima_user_DllExport CollisionEvent& operator =(
+                    CollisionEvent&& x) noexcept;
+
+            /*!
+             * @brief Comparison operator.
+             * @param x carla_msgs::msg::CollisionEvent object to compare.
+             */
+            eProsima_user_DllExport bool operator ==(
+                    const CollisionEvent& x) const;
+
+            /*!
+             * @brief Comparison operator.
+             * @param x carla_msgs::msg::CollisionEvent object to compare.
+             */
+            eProsima_user_DllExport bool operator !=(
+                    const CollisionEvent& x) const;
+
+            /*!
+             * @brief This function copies the value in member header
+             * @param _header New value to be copied in member header
+             */
+            eProsima_user_DllExport void header(
+                    const std_msgs::msg::Header& _header);
+
+            /*!
+             * @brief This function moves the value in member header
+             * @param _header New value to be moved in member header
+             */
+            eProsima_user_DllExport void header(
+                    std_msgs::msg::Header&& _header);
+
+            /*!
+             * @brief This function returns a constant reference to member header
+             * @return Constant reference to member header
+             */
+            eProsima_user_DllExport const std_msgs::msg::Header& header() const;
+
+            /*!
+             * @brief This function returns a reference to member header
+             * @return Reference to member header
+             */
+            eProsima_user_DllExport std_msgs::msg::Header& header();
+            /*!
+             * @brief This function sets a value in member other_actor_id
+             * @param _other_actor_id New value for member other_actor_id
+             */
+            eProsima_user_DllExport void other_actor_id(
+                    uint32_t _other_actor_id);
+
+            /*!
+             * @brief This function returns the value of member other_actor_id
+             * @return Value of member other_actor_id
+             */
+            eProsima_user_DllExport uint32_t other_actor_id() const;
+
+            /*!
+             * @brief This function returns a reference to member other_actor_id
+             * @return Reference to member other_actor_id
+             */
+            eProsima_user_DllExport uint32_t& other_actor_id();
+
+            /*!
+             * @brief This function copies the value in member normal_impulse
+             * @param _normal_impulse New value to be copied in member normal_impulse
+             */
+            eProsima_user_DllExport void normal_impulse(
+                    const geometry_msgs::msg::Vector3& _normal_impulse);
+
+            /*!
+             * @brief This function moves the value in member normal_impulse
+             * @param _normal_impulse New value to be moved in member normal_impulse
+             */
+            eProsima_user_DllExport void normal_impulse(
+                    geometry_msgs::msg::Vector3&& _normal_impulse);
+
+            /*!
+             * @brief This function returns a constant reference to member normal_impulse
+             * @return Constant reference to member normal_impulse
+             */
+            eProsima_user_DllExport const geometry_msgs::msg::Vector3& normal_impulse() const;
+
+            /*!
+             * @brief This function returns a reference to member normal_impulse
+             * @return Reference to member normal_impulse
+             */
+            eProsima_user_DllExport geometry_msgs::msg::Vector3& normal_impulse();
+
+            /*!
+            * @brief This function returns the maximum serialized size of an object
+            * depending on the buffer alignment.
+            * @param current_alignment Buffer alignment.
+            * @return Maximum serialized size.
+            */
+            eProsima_user_DllExport static size_t getMaxCdrSerializedSize(
+                    size_t current_alignment = 0);
+
+            /*!
+             * @brief This function returns the serialized size of a data depending on the buffer alignment.
+             * @param data Data which is calculated its serialized size.
+             * @param current_alignment Buffer alignment.
+             * @return Serialized size.
+             */
+            eProsima_user_DllExport static size_t getCdrSerializedSize(
+                    const carla_msgs::msg::CollisionEvent& data,
+                    size_t current_alignment = 0);
+
+            /*!
+             * @brief This function serializes an object using CDR serialization.
+             * @param cdr CDR serialization object.
+             */
+            eProsima_user_DllExport void serialize(
+                    eprosima::fastcdr::Cdr& cdr) const;
+
+            /*!
+             * @brief This function deserializes an object using CDR serialization.
+             * @param cdr CDR serialization object.
+             */
+            eProsima_user_DllExport void deserialize(
+                    eprosima::fastcdr::Cdr& cdr);
+
+            /*!
+             * @brief This function returns the maximum serialized size of the Key of an object
+             * depending on the buffer alignment.
+             * @param current_alignment Buffer alignment.
+             * @return Maximum serialized size.
+             */
+            eProsima_user_DllExport static size_t getKeyMaxCdrSerializedSize(
+                    size_t current_alignment = 0);
+
+            /*!
+             * @brief This function tells you if the Key has been defined for this type
+             */
+            eProsima_user_DllExport static bool isKeyDefined();
+
+            /*!
+             * @brief This function serializes the key members of an object using CDR serialization.
+             * @param cdr CDR serialization object.
+             */
+            eProsima_user_DllExport void serializeKey(
+                    eprosima::fastcdr::Cdr& cdr) const;
+
+        private:
+            std_msgs::msg::Header m_header;
+            uint32_t m_other_actor_id;
+            geometry_msgs::msg::Vector3 m_normal_impulse;
+        };
+    } // namespace msg
+} // namespace carla_msgs
+
+#endif // _FAST_DDS_GENERATED_CARLA_MSGS_MSG_CARLACOLLISIONEVENT_H_

LibCarla/source/carla/ros2/types/CarlaCollisionEventPubSubTypes.cpp

@@ -0,0 +1,172 @@
+// Copyright 2016 Proyectos y Sistemas de Mantenimiento SL (eProsima).
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+/*!
+ * @file CarlaCollisionEventPubSubTypes.cpp
+ * This header file contains the implementation of the serialization functions.
+ *
+ * This file was generated by the tool fastcdrgen.
+ */
+
+#include <fastcdr/FastBuffer.h>
+#include <fastcdr/Cdr.h>
+
+#include "CarlaCollisionEventPubSubTypes.h"
+
+using SerializedPayload_t = eprosima::fastrtps::rtps::SerializedPayload_t;
+using InstanceHandle_t = eprosima::fastrtps::rtps::InstanceHandle_t;
+
+namespace carla_msgs {
+    namespace msg {
+        CollisionEventPubSubType::CollisionEventPubSubType()
+        {
+            setName("carla_msgs::msg::dds_::CollisionEvent_");
+            auto type_size = CollisionEvent::getMaxCdrSerializedSize();
+            type_size += eprosima::fastcdr::Cdr::alignment(type_size, 4); /* possible submessage alignment */
+            m_typeSize = static_cast<uint32_t>(type_size) + 4; /*encapsulation*/
+            m_isGetKeyDefined = CollisionEvent::isKeyDefined();
+            size_t keyLength = CollisionEvent::getKeyMaxCdrSerializedSize() > 16 ?
+                    CollisionEvent::getKeyMaxCdrSerializedSize() : 16;
+            m_keyBuffer = reinterpret_cast<unsigned char*>(malloc(keyLength));
+            memset(m_keyBuffer, 0, keyLength);
+        }
+
+        CollisionEventPubSubType::~CollisionEventPubSubType()
+        {
+            if (m_keyBuffer != nullptr)
+            {
+                free(m_keyBuffer);
+            }
+        }
+
+        bool CollisionEventPubSubType::serialize(
+                void* data,
+                SerializedPayload_t* payload)
+        {
+            CollisionEvent* p_type = static_cast<CollisionEvent*>(data);
+
+            // Object that manages the raw buffer.
+            eprosima::fastcdr::FastBuffer fastbuffer(reinterpret_cast<char*>(payload->data), payload->max_size);
+            // Object that serializes the data.
+            eprosima::fastcdr::Cdr ser(fastbuffer, eprosima::fastcdr::Cdr::DEFAULT_ENDIAN, eprosima::fastcdr::Cdr::DDS_CDR);
+            payload->encapsulation = ser.endianness() == eprosima::fastcdr::Cdr::BIG_ENDIANNESS ? CDR_BE : CDR_LE;
+            // Serialize encapsulation
+            ser.serialize_encapsulation();
+
+            try
+            {
+                // Serialize the object.
+                p_type->serialize(ser);
+            }
+            catch (eprosima::fastcdr::exception::NotEnoughMemoryException& /*exception*/)
+            {
+                return false;
+            }
+
+            // Get the serialized length
+            payload->length = static_cast<uint32_t>(ser.getSerializedDataLength());
+            return true;
+        }
+
+        bool CollisionEventPubSubType::deserialize(
+                SerializedPayload_t* payload,
+                void* data)
+        {
+            try
+            {
+                //Convert DATA to pointer of your type
+                CollisionEvent* p_type = static_cast<CollisionEvent*>(data);
+
+                // Object that manages the raw buffer.
+                eprosima::fastcdr::FastBuffer fastbuffer(reinterpret_cast<char*>(payload->data), payload->length);
+
+                // Object that deserializes the data.
+                eprosima::fastcdr::Cdr deser(fastbuffer, eprosima::fastcdr::Cdr::DEFAULT_ENDIAN, eprosima::fastcdr::Cdr::DDS_CDR);
+
+                // Deserialize encapsulation.
+                deser.read_encapsulation();
+                payload->encapsulation = deser.endianness() == eprosima::fastcdr::Cdr::BIG_ENDIANNESS ? CDR_BE : CDR_LE;
+
+                // Deserialize the object.
+                p_type->deserialize(deser);
+            }
+            catch (eprosima::fastcdr::exception::NotEnoughMemoryException& /*exception*/)
+            {
+                return false;
+            }
+
+            return true;
+        }
+
+        std::function<uint32_t()> CollisionEventPubSubType::getSerializedSizeProvider(
+                void* data)
+        {
+            return [data]() -> uint32_t
+                   {
+                       return static_cast<uint32_t>(type::getCdrSerializedSize(*static_cast<CollisionEvent*>(data))) +
+                              4u /*encapsulation*/;
+                   };
+        }
+
+        void* CollisionEventPubSubType::createData()
+        {
+            return reinterpret_cast<void*>(new CollisionEvent());
+        }
+
+        void CollisionEventPubSubType::deleteData(
+                void* data)
+        {
+            delete(reinterpret_cast<CollisionEvent*>(data));
+        }
+
+        bool CollisionEventPubSubType::getKey(
+                void* data,
+                InstanceHandle_t* handle,
+                bool force_md5)
+        {
+            if (!m_isGetKeyDefined)
+            {
+                return false;
+            }
+
+            CollisionEvent* p_type = static_cast<CollisionEvent*>(data);
+
+            // Object that manages the raw buffer.
+            eprosima::fastcdr::FastBuffer fastbuffer(reinterpret_cast<char*>(m_keyBuffer),
+                    CollisionEvent::getKeyMaxCdrSerializedSize());
+
+            // Object that serializes the data.
+            eprosima::fastcdr::Cdr ser(fastbuffer, eprosima::fastcdr::Cdr::BIG_ENDIANNESS);
+            p_type->serializeKey(ser);
+            if (force_md5 || CollisionEvent::getKeyMaxCdrSerializedSize() > 16)
+            {
+                m_md5.init();
+                m_md5.update(m_keyBuffer, static_cast<unsigned int>(ser.getSerializedDataLength()));
+                m_md5.finalize();
+                for (uint8_t i = 0; i < 16; ++i)
+                {
+                    handle->value[i] = m_md5.digest[i];
+                }
+            }
+            else
+            {
+                for (uint8_t i = 0; i < 16; ++i)
+                {
+                    handle->value[i] = m_keyBuffer[i];
+                }
+            }
+            return true;
+        }
+    } //End of namespace msg
+} //End of namespace carla_msgs

LibCarla/source/carla/ros2/types/CarlaCollisionEventPubSubTypes.h

@@ -0,0 +1,111 @@
+// Copyright 2016 Proyectos y Sistemas de Mantenimiento SL (eProsima).
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+/*!
+ * @file CarlaCollisionEventPubSubTypes.h
+ * This header file contains the declaration of the serialization functions.
+ *
+ * This file was generated by the tool fastcdrgen.
+ */
+
+
+#ifndef _FAST_DDS_GENERATED_CARLA_MSGS_MSG_CARLACOLLISIONEVENT_PUBSUBTYPES_H_
+#define _FAST_DDS_GENERATED_CARLA_MSGS_MSG_CARLACOLLISIONEVENT_PUBSUBTYPES_H_
+
+#include <fastdds/dds/topic/TopicDataType.hpp>
+#include <fastrtps/utils/md5.h>
+
+#include "CarlaCollisionEvent.h"
+#include "Vector3PubSubTypes.h"
+#include "HeaderPubSubTypes.h"
+
+#if !defined(GEN_API_VER) || (GEN_API_VER != 1)
+#error \
+    Generated CarlaCollisionEvent is not compatible with current installed Fast DDS. Please, regenerate it with fastddsgen.
+#endif  // GEN_API_VER
+
+namespace carla_msgs
+{
+    namespace msg
+    {
+
+        /*!
+         * @brief This class represents the TopicDataType of the type CollisionEvent defined by the user in the IDL file.
+         * @ingroup CARLACOLLISIONEVENT
+         */
+        class CollisionEventPubSubType : public eprosima::fastdds::dds::TopicDataType
+        {
+        public:
+
+            typedef CollisionEvent type;
+
+            eProsima_user_DllExport CollisionEventPubSubType();
+
+            eProsima_user_DllExport virtual ~CollisionEventPubSubType() override;
+
+            eProsima_user_DllExport virtual bool serialize(
+                    void* data,
+                    eprosima::fastrtps::rtps::SerializedPayload_t* payload) override;
+
+            eProsima_user_DllExport virtual bool deserialize(
+                    eprosima::fastrtps::rtps::SerializedPayload_t* payload,
+                    void* data) override;
+
+            eProsima_user_DllExport virtual std::function<uint32_t()> getSerializedSizeProvider(
+                    void* data) override;
+
+            eProsima_user_DllExport virtual bool getKey(
+                    void* data,
+                    eprosima::fastrtps::rtps::InstanceHandle_t* ihandle,
+                    bool force_md5 = false) override;
+
+            eProsima_user_DllExport virtual void* createData() override;
+
+            eProsima_user_DllExport virtual void deleteData(
+                    void* data) override;
+
+        #ifdef TOPIC_DATA_TYPE_API_HAS_IS_BOUNDED
+            eProsima_user_DllExport inline bool is_bounded() const override
+            {
+                return false;
+            }
+
+        #endif  // TOPIC_DATA_TYPE_API_HAS_IS_BOUNDED
+
+        #ifdef TOPIC_DATA_TYPE_API_HAS_IS_PLAIN
+            eProsima_user_DllExport inline bool is_plain() const override
+            {
+                return false;
+            }
+
+        #endif  // TOPIC_DATA_TYPE_API_HAS_IS_PLAIN
+
+        #ifdef TOPIC_DATA_TYPE_API_HAS_CONSTRUCT_SAMPLE
+            eProsima_user_DllExport inline bool construct_sample(
+                    void* memory) const override
+            {
+                (void)memory;
+                return false;
+            }
+
+        #endif  // TOPIC_DATA_TYPE_API_HAS_CONSTRUCT_SAMPLE
+
+            MD5 m_md5;
+            unsigned char* m_keyBuffer;
+
+        };
+    }
+}
+
+#endif // _FAST_DDS_GENERATED_CARLA_MSGS_MSG_CARLACOLLISIONEVENT_PUBSUBTYPES_H_

LibCarla/source/carla/ros2/types/CarlaEgoVehicleControl.cpp

@@ -0,0 +1,435 @@
+// Copyright 2016 Proyectos y Sistemas de Mantenimiento SL (eProsima).
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+/*!
+ * @file CarlaEgoCarlaEgoVehicleControl.cpp
+ * This source file contains the definition of the described types in the IDL file.
+ *
+ * This file was generated by the tool gen.
+ */
+
+#ifdef _WIN32
+// Remove linker warning LNK4221 on Visual Studio
+namespace {
+char dummy;
+}  // namespace
+#endif  // _WIN32
+
+#include "CarlaEgoVehicleControl.h"
+#include <fastcdr/Cdr.h>
+
+#include <fastcdr/exceptions/BadParamException.h>
+using namespace eprosima::fastcdr::exception;
+
+#include <utility>
+
+#define builtin_interfaces_msg_Time_max_cdr_typesize 8ULL;
+#define std_msgs_msg_Header_max_cdr_typesize 268ULL;
+#define carla_msgs_msg_CarlaEgoVehicleControl_max_cdr_typesize 289ULL;
+#define builtin_interfaces_msg_Time_max_key_cdr_typesize 0ULL;
+#define std_msgs_msg_Header_max_key_cdr_typesize 0ULL;
+#define carla_msgs_msg_CarlaEgoVehicleControl_max_key_cdr_typesize 0ULL;
+
+carla_msgs::msg::CarlaEgoVehicleControl::CarlaEgoVehicleControl()
+{
+    // std_msgs::msg::Header m_header
+
+    // float m_throttle
+    m_throttle = 0.0;
+    // float m_steer
+    m_steer = 0.0;
+    // float m_brake
+    m_brake = 0.0;
+    // boolean m_hand_brake
+    m_hand_brake = false;
+    // boolean m_reverse
+    m_reverse = false;
+    // long m_gear
+    m_gear = 0;
+    // boolean m_manual_gear_shift
+    m_manual_gear_shift = false;
+
+}
+
+carla_msgs::msg::CarlaEgoVehicleControl::~CarlaEgoVehicleControl()
+{
+}
+
+carla_msgs::msg::CarlaEgoVehicleControl::CarlaEgoVehicleControl(
+        const CarlaEgoVehicleControl& x)
+{
+    m_header = x.m_header;
+    m_throttle = x.m_throttle;
+    m_steer = x.m_steer;
+    m_brake = x.m_brake;
+    m_hand_brake = x.m_hand_brake;
+    m_reverse = x.m_reverse;
+    m_gear = x.m_gear;
+    m_manual_gear_shift = x.m_manual_gear_shift;
+}
+
+carla_msgs::msg::CarlaEgoVehicleControl::CarlaEgoVehicleControl(
+        CarlaEgoVehicleControl&& x) noexcept
+{
+    m_header = std::move(x.m_header);
+    m_throttle = x.m_throttle;
+    m_steer = x.m_steer;
+    m_brake = x.m_brake;
+    m_hand_brake = x.m_hand_brake;
+    m_reverse = x.m_reverse;
+    m_gear = x.m_gear;
+    m_manual_gear_shift = x.m_manual_gear_shift;
+}
+
+carla_msgs::msg::CarlaEgoVehicleControl& carla_msgs::msg::CarlaEgoVehicleControl::operator =(
+        const CarlaEgoVehicleControl& x)
+{
+    m_header = x.m_header;
+    m_throttle = x.m_throttle;
+    m_steer = x.m_steer;
+    m_brake = x.m_brake;
+    m_hand_brake = x.m_hand_brake;
+    m_reverse = x.m_reverse;
+    m_gear = x.m_gear;
+    m_manual_gear_shift = x.m_manual_gear_shift;
+
+    return *this;
+}
+
+carla_msgs::msg::CarlaEgoVehicleControl& carla_msgs::msg::CarlaEgoVehicleControl::operator =(
+        CarlaEgoVehicleControl&& x) noexcept
+{
+    m_header = std::move(x.m_header);
+    m_throttle = x.m_throttle;
+    m_steer = x.m_steer;
+    m_brake = x.m_brake;
+    m_hand_brake = x.m_hand_brake;
+    m_reverse = x.m_reverse;
+    m_gear = x.m_gear;
+    m_manual_gear_shift = x.m_manual_gear_shift;
+
+    return *this;
+}
+
+bool carla_msgs::msg::CarlaEgoVehicleControl::operator ==(
+        const CarlaEgoVehicleControl& x) const
+{
+    return (m_header == x.m_header && m_throttle == x.m_throttle && m_steer == x.m_steer && m_brake == x.m_brake && m_hand_brake == x.m_hand_brake && m_reverse == x.m_reverse && m_gear == x.m_gear && m_manual_gear_shift == x.m_manual_gear_shift);
+}
+
+bool carla_msgs::msg::CarlaEgoVehicleControl::operator !=(
+        const CarlaEgoVehicleControl& x) const
+{
+    return !(*this == x);
+}
+
+size_t carla_msgs::msg::CarlaEgoVehicleControl::getMaxCdrSerializedSize(
+        size_t current_alignment)
+{
+    static_cast<void>(current_alignment);
+    return carla_msgs_msg_CarlaEgoVehicleControl_max_cdr_typesize;
+}
+
+size_t carla_msgs::msg::CarlaEgoVehicleControl::getCdrSerializedSize(
+        const carla_msgs::msg::CarlaEgoVehicleControl& data,
+        size_t current_alignment)
+{
+    size_t initial_alignment = current_alignment;
+    current_alignment += std_msgs::msg::Header::getCdrSerializedSize(data.header(), current_alignment);
+    current_alignment += 4 + eprosima::fastcdr::Cdr::alignment(current_alignment, 4);
+    current_alignment += 4 + eprosima::fastcdr::Cdr::alignment(current_alignment, 4);
+    current_alignment += 4 + eprosima::fastcdr::Cdr::alignment(current_alignment, 4);
+    current_alignment += 1 + eprosima::fastcdr::Cdr::alignment(current_alignment, 1);
+    current_alignment += 1 + eprosima::fastcdr::Cdr::alignment(current_alignment, 1);
+    current_alignment += 4 + eprosima::fastcdr::Cdr::alignment(current_alignment, 4);
+    current_alignment += 1 + eprosima::fastcdr::Cdr::alignment(current_alignment, 1);
+    return current_alignment - initial_alignment;
+}
+
+void carla_msgs::msg::CarlaEgoVehicleControl::serialize(
+        eprosima::fastcdr::Cdr& scdr) const
+{
+    scdr << m_header;
+    scdr << m_throttle;
+    scdr << m_steer;
+    scdr << m_brake;
+    scdr << m_hand_brake;
+    scdr << m_reverse;
+    scdr << m_gear;
+    scdr << m_manual_gear_shift;
+}
+
+void carla_msgs::msg::CarlaEgoVehicleControl::deserialize(
+        eprosima::fastcdr::Cdr& dcdr)
+{
+    dcdr >> m_header;
+    dcdr >> m_throttle;
+    dcdr >> m_steer;
+    dcdr >> m_brake;
+    dcdr >> m_hand_brake;
+    dcdr >> m_reverse;
+    dcdr >> m_gear;
+    dcdr >> m_manual_gear_shift;
+}
+
+/*!
+ * @brief This function copies the value in member header
+ * @param _header New value to be copied in member header
+ */
+void carla_msgs::msg::CarlaEgoVehicleControl::header(
+        const std_msgs::msg::Header& _header)
+{
+    m_header = _header;
+}
+
+/*!
+ * @brief This function moves the value in member header
+ * @param _header New value to be moved in member header
+ */
+void carla_msgs::msg::CarlaEgoVehicleControl::header(
+        std_msgs::msg::Header&& _header)
+{
+    m_header = std::move(_header);
+}
+
+/*!
+ * @brief This function returns a constant reference to member header
+ * @return Constant reference to member header
+ */
+const std_msgs::msg::Header& carla_msgs::msg::CarlaEgoVehicleControl::header() const
+{
+    return m_header;
+}
+
+/*!
+ * @brief This function returns a reference to member header
+ * @return Reference to member header
+ */
+std_msgs::msg::Header& carla_msgs::msg::CarlaEgoVehicleControl::header()
+{
+    return m_header;
+}
+/*!
+ * @brief This function sets a value in member throttle
+ * @param _throttle New value for member throttle
+ */
+void carla_msgs::msg::CarlaEgoVehicleControl::throttle(
+        float _throttle)
+{
+    m_throttle = _throttle;
+}
+
+/*!
+ * @brief This function returns the value of member throttle
+ * @return Value of member throttle
+ */
+float carla_msgs::msg::CarlaEgoVehicleControl::throttle() const
+{
+    return m_throttle;
+}
+
+/*!
+ * @brief This function returns a reference to member throttle
+ * @return Reference to member throttle
+ */
+float& carla_msgs::msg::CarlaEgoVehicleControl::throttle()
+{
+    return m_throttle;
+}
+
+/*!
+ * @brief This function sets a value in member steer
+ * @param _steer New value for member steer
+ */
+void carla_msgs::msg::CarlaEgoVehicleControl::steer(
+        float _steer)
+{
+    m_steer = _steer;
+}
+
+/*!
+ * @brief This function returns the value of member steer
+ * @return Value of member steer
+ */
+float carla_msgs::msg::CarlaEgoVehicleControl::steer() const
+{
+    return m_steer;
+}
+
+/*!
+ * @brief This function returns a reference to member steer
+ * @return Reference to member steer
+ */
+float& carla_msgs::msg::CarlaEgoVehicleControl::steer()
+{
+    return m_steer;
+}
+
+/*!
+ * @brief This function sets a value in member brake
+ * @param _brake New value for member brake
+ */
+void carla_msgs::msg::CarlaEgoVehicleControl::brake(
+        float _brake)
+{
+    m_brake = _brake;
+}
+
+/*!
+ * @brief This function returns the value of member brake
+ * @return Value of member brake
+ */
+float carla_msgs::msg::CarlaEgoVehicleControl::brake() const
+{
+    return m_brake;
+}
+
+/*!
+ * @brief This function returns a reference to member brake
+ * @return Reference to member brake
+ */
+float& carla_msgs::msg::CarlaEgoVehicleControl::brake()
+{
+    return m_brake;
+}
+
+/*!
+ * @brief This function sets a value in member hand_brake
+ * @param _hand_brake New value for member hand_brake
+ */
+void carla_msgs::msg::CarlaEgoVehicleControl::hand_brake(
+        bool _hand_brake)
+{
+    m_hand_brake = _hand_brake;
+}
+
+/*!
+ * @brief This function returns the value of member hand_brake
+ * @return Value of member hand_brake
+ */
+bool carla_msgs::msg::CarlaEgoVehicleControl::hand_brake() const
+{
+    return m_hand_brake;
+}
+
+/*!
+ * @brief This function returns a reference to member hand_brake
+ * @return Reference to member hand_brake
+ */
+bool& carla_msgs::msg::CarlaEgoVehicleControl::hand_brake()
+{
+    return m_hand_brake;
+}
+
+/*!
+ * @brief This function sets a value in member reverse
+ * @param _reverse New value for member reverse
+ */
+void carla_msgs::msg::CarlaEgoVehicleControl::reverse(
+        bool _reverse)
+{
+    m_reverse = _reverse;
+}
+
+/*!
+ * @brief This function returns the value of member reverse
+ * @return Value of member reverse
+ */
+bool carla_msgs::msg::CarlaEgoVehicleControl::reverse() const
+{
+    return m_reverse;
+}
+
+/*!
+ * @brief This function returns a reference to member reverse
+ * @return Reference to member reverse
+ */
+bool& carla_msgs::msg::CarlaEgoVehicleControl::reverse()
+{
+    return m_reverse;
+}
+
+/*!
+ * @brief This function sets a value in member gear
+ * @param _gear New value for member gear
+ */
+void carla_msgs::msg::CarlaEgoVehicleControl::gear(
+        int32_t _gear)
+{
+    m_gear = _gear;
+}
+
+/*!
+ * @brief This function returns the value of member gear
+ * @return Value of member gear
+ */
+int32_t carla_msgs::msg::CarlaEgoVehicleControl::gear() const
+{
+    return m_gear;
+}
+
+/*!
+ * @brief This function returns a reference to member gear
+ * @return Reference to member gear
+ */
+int32_t& carla_msgs::msg::CarlaEgoVehicleControl::gear()
+{
+    return m_gear;
+}
+
+/*!
+ * @brief This function sets a value in member manual_gear_shift
+ * @param _manual_gear_shift New value for member manual_gear_shift
+ */
+void carla_msgs::msg::CarlaEgoVehicleControl::manual_gear_shift(
+        bool _manual_gear_shift)
+{
+    m_manual_gear_shift = _manual_gear_shift;
+}
+
+/*!
+ * @brief This function returns the value of member manual_gear_shift
+ * @return Value of member manual_gear_shift
+ */
+bool carla_msgs::msg::CarlaEgoVehicleControl::manual_gear_shift() const
+{
+    return m_manual_gear_shift;
+}
+
+/*!
+ * @brief This function returns a reference to member manual_gear_shift
+ * @return Reference to member manual_gear_shift
+ */
+bool& carla_msgs::msg::CarlaEgoVehicleControl::manual_gear_shift()
+{
+    return m_manual_gear_shift;
+}
+
+size_t carla_msgs::msg::CarlaEgoVehicleControl::getKeyMaxCdrSerializedSize(
+        size_t current_alignment)
+{
+    static_cast<void>(current_alignment);
+    return carla_msgs_msg_CarlaEgoVehicleControl_max_key_cdr_typesize;
+}
+
+bool carla_msgs::msg::CarlaEgoVehicleControl::isKeyDefined()
+{
+    return false;
+}
+
+void carla_msgs::msg::CarlaEgoVehicleControl::serializeKey(
+        eprosima::fastcdr::Cdr& scdr) const
+{
+    (void) scdr;
+}

LibCarla/source/carla/ros2/types/CarlaEgoVehicleControl.h

@@ -0,0 +1,354 @@
+// Copyright 2016 Proyectos y Sistemas de Mantenimiento SL (eProsima).
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+/*!
+ * @file CarlaEgoCarlaEgoVehicleControl.h
+ * This header file contains the declaration of the described types in the IDL file.
+ *
+ * This file was generated by the tool gen.
+ */
+
+#ifndef _FAST_DDS_GENERATED_CARLA_MSGS_MSG_CARLAEGOCarlaEgoVehicleControl_H_
+#define _FAST_DDS_GENERATED_CARLA_MSGS_MSG_CARLAEGOCarlaEgoVehicleControl_H_
+
+#include "Header.h"
+
+#include <fastrtps/utils/fixed_size_string.hpp>
+
+#include <stdint.h>
+#include <array>
+#include <string>
+#include <vector>
+#include <map>
+#include <bitset>
+
+#if defined(_WIN32)
+#if defined(EPROSIMA_USER_DLL_EXPORT)
+#define eProsima_user_DllExport __declspec( dllexport )
+#else
+#define eProsima_user_DllExport
+#endif  // EPROSIMA_USER_DLL_EXPORT
+#else
+#define eProsima_user_DllExport
+#endif  // _WIN32
+
+#if defined(_WIN32)
+#if defined(EPROSIMA_USER_DLL_EXPORT)
+#if defined(CarlaEgoCarlaEgoVehicleControl_SOURCE)
+#define CarlaEgoCarlaEgoVehicleControl_DllAPI __declspec( dllexport )
+#else
+#define CarlaEgoCarlaEgoVehicleControl_DllAPI __declspec( dllimport )
+#endif // CarlaEgoCarlaEgoVehicleControl_SOURCE
+#else
+#define CarlaEgoCarlaEgoVehicleControl_DllAPI
+#endif  // EPROSIMA_USER_DLL_EXPORT
+#else
+#define CarlaEgoCarlaEgoVehicleControl_DllAPI
+#endif // _WIN32
+
+namespace eprosima {
+namespace fastcdr {
+class Cdr;
+} // namespace fastcdr
+} // namespace eprosima
+
+namespace carla_msgs {
+    namespace msg {
+        /*!
+         * @brief This class represents the structure CarlaEgoVehicleControl defined by the user in the IDL file.
+         * @ingroup CarlaEgoVehicleControl
+         */
+        class CarlaEgoVehicleControl
+        {
+        public:
+
+            /*!
+             * @brief Default constructor.
+             */
+            eProsima_user_DllExport CarlaEgoVehicleControl();
+
+            /*!
+             * @brief Default destructor.
+             */
+            eProsima_user_DllExport ~CarlaEgoVehicleControl();
+
+            /*!
+             * @brief Copy constructor.
+             * @param x Reference to the object carla_msgs::msg::CarlaEgoVehicleControl that will be copied.
+             */
+            eProsima_user_DllExport CarlaEgoVehicleControl(
+                    const CarlaEgoVehicleControl& x);
+
+            /*!
+             * @brief Move constructor.
+             * @param x Reference to the object carla_msgs::msg::CarlaEgoVehicleControl that will be copied.
+             */
+            eProsima_user_DllExport CarlaEgoVehicleControl(
+                    CarlaEgoVehicleControl&& x) noexcept;
+
+            /*!
+             * @brief Copy assignment.
+             * @param x Reference to the object carla_msgs::msg::CarlaEgoVehicleControl that will be copied.
+             */
+            eProsima_user_DllExport CarlaEgoVehicleControl& operator =(
+                    const CarlaEgoVehicleControl& x);
+
+            /*!
+             * @brief Move assignment.
+             * @param x Reference to the object carla_msgs::msg::CarlaEgoVehicleControl that will be copied.
+             */
+            eProsima_user_DllExport CarlaEgoVehicleControl& operator =(
+                    CarlaEgoVehicleControl&& x) noexcept;
+
+            /*!
+             * @brief Comparison operator.
+             * @param x carla_msgs::msg::CarlaEgoVehicleControl object to compare.
+             */
+            eProsima_user_DllExport bool operator ==(
+                    const CarlaEgoVehicleControl& x) const;
+
+            /*!
+             * @brief Comparison operator.
+             * @param x carla_msgs::msg::CarlaEgoVehicleControl object to compare.
+             */
+            eProsima_user_DllExport bool operator !=(
+                    const CarlaEgoVehicleControl& x) const;
+
+            /*!
+             * @brief This function copies the value in member header
+             * @param _header New value to be copied in member header
+             */
+            eProsima_user_DllExport void header(
+                    const std_msgs::msg::Header& _header);
+
+            /*!
+             * @brief This function moves the value in member header
+             * @param _header New value to be moved in member header
+             */
+            eProsima_user_DllExport void header(
+                    std_msgs::msg::Header&& _header);
+
+            /*!
+             * @brief This function returns a constant reference to member header
+             * @return Constant reference to member header
+             */
+            eProsima_user_DllExport const std_msgs::msg::Header& header() const;
+
+            /*!
+             * @brief This function returns a reference to member header
+             * @return Reference to member header
+             */
+            eProsima_user_DllExport std_msgs::msg::Header& header();
+            /*!
+             * @brief This function sets a value in member throttle
+             * @param _throttle New value for member throttle
+             */
+            eProsima_user_DllExport void throttle(
+                    float _throttle);
+
+            /*!
+             * @brief This function returns the value of member throttle
+             * @return Value of member throttle
+             */
+            eProsima_user_DllExport float throttle() const;
+
+            /*!
+             * @brief This function returns a reference to member throttle
+             * @return Reference to member throttle
+             */
+            eProsima_user_DllExport float& throttle();
+
+            /*!
+             * @brief This function sets a value in member steer
+             * @param _steer New value for member steer
+             */
+            eProsima_user_DllExport void steer(
+                    float _steer);
+
+            /*!
+             * @brief This function returns the value of member steer
+             * @return Value of member steer
+             */
+            eProsima_user_DllExport float steer() const;
+
+            /*!
+             * @brief This function returns a reference to member steer
+             * @return Reference to member steer
+             */
+            eProsima_user_DllExport float& steer();
+
+            /*!
+             * @brief This function sets a value in member brake
+             * @param _brake New value for member brake
+             */
+            eProsima_user_DllExport void brake(
+                    float _brake);
+
+            /*!
+             * @brief This function returns the value of member brake
+             * @return Value of member brake
+             */
+            eProsima_user_DllExport float brake() const;
+
+            /*!
+             * @brief This function returns a reference to member brake
+             * @return Reference to member brake
+             */
+            eProsima_user_DllExport float& brake();
+
+            /*!
+             * @brief This function sets a value in member hand_brake
+             * @param _hand_brake New value for member hand_brake
+             */
+            eProsima_user_DllExport void hand_brake(
+                    bool _hand_brake);
+
+            /*!
+             * @brief This function returns the value of member hand_brake
+             * @return Value of member hand_brake
+             */
+            eProsima_user_DllExport bool hand_brake() const;
+
+            /*!
+             * @brief This function returns a reference to member hand_brake
+             * @return Reference to member hand_brake
+             */
+            eProsima_user_DllExport bool& hand_brake();
+
+            /*!
+             * @brief This function sets a value in member reverse
+             * @param _reverse New value for member reverse
+             */
+            eProsima_user_DllExport void reverse(
+                    bool _reverse);
+
+            /*!
+             * @brief This function returns the value of member reverse
+             * @return Value of member reverse
+             */
+            eProsima_user_DllExport bool reverse() const;
+
+            /*!
+             * @brief This function returns a reference to member reverse
+             * @return Reference to member reverse
+             */
+            eProsima_user_DllExport bool& reverse();
+
+            /*!
+             * @brief This function sets a value in member gear
+             * @param _gear New value for member gear
+             */
+            eProsima_user_DllExport void gear(
+                    int32_t _gear);
+
+            /*!
+             * @brief This function returns the value of member gear
+             * @return Value of member gear
+             */
+            eProsima_user_DllExport int32_t gear() const;
+
+            /*!
+             * @brief This function returns a reference to member gear
+             * @return Reference to member gear
+             */
+            eProsima_user_DllExport int32_t& gear();
+
+            /*!
+             * @brief This function sets a value in member manual_gear_shift
+             * @param _manual_gear_shift New value for member manual_gear_shift
+             */
+            eProsima_user_DllExport void manual_gear_shift(
+                    bool _manual_gear_shift);
+
+            /*!
+             * @brief This function returns the value of member manual_gear_shift
+             * @return Value of member manual_gear_shift
+             */
+            eProsima_user_DllExport bool manual_gear_shift() const;
+
+            /*!
+             * @brief This function returns a reference to member manual_gear_shift
+             * @return Reference to member manual_gear_shift
+             */
+            eProsima_user_DllExport bool& manual_gear_shift();
+
+            /*!
+            * @brief This function returns the maximum serialized size of an object
+            * depending on the buffer alignment.
+            * @param current_alignment Buffer alignment.
+            * @return Maximum serialized size.
+            */
+            eProsima_user_DllExport static size_t getMaxCdrSerializedSize(
+                    size_t current_alignment = 0);
+
+            /*!
+             * @brief This function returns the serialized size of a data depending on the buffer alignment.
+             * @param data Data which is calculated its serialized size.
+             * @param current_alignment Buffer alignment.
+             * @return Serialized size.
+             */
+            eProsima_user_DllExport static size_t getCdrSerializedSize(
+                    const carla_msgs::msg::CarlaEgoVehicleControl& data,
+                    size_t current_alignment = 0);
+
+            /*!
+             * @brief This function serializes an object using CDR serialization.
+             * @param cdr CDR serialization object.
+             */
+            eProsima_user_DllExport void serialize(
+                    eprosima::fastcdr::Cdr& cdr) const;
+
+            /*!
+             * @brief This function deserializes an object using CDR serialization.
+             * @param cdr CDR serialization object.
+             */
+            eProsima_user_DllExport void deserialize(
+                    eprosima::fastcdr::Cdr& cdr);
+
+            /*!
+             * @brief This function returns the maximum serialized size of the Key of an object
+             * depending on the buffer alignment.
+             * @param current_alignment Buffer alignment.
+             * @return Maximum serialized size.
+             */
+            eProsima_user_DllExport static size_t getKeyMaxCdrSerializedSize(
+                    size_t current_alignment = 0);
+
+            /*!
+             * @brief This function tells you if the Key has been defined for this type
+             */
+            eProsima_user_DllExport static bool isKeyDefined();
+
+            /*!
+             * @brief This function serializes the key members of an object using CDR serialization.
+             * @param cdr CDR serialization object.
+             */
+            eProsima_user_DllExport void serializeKey(
+                    eprosima::fastcdr::Cdr& cdr) const;
+
+        private:
+            std_msgs::msg::Header m_header;
+            float m_throttle;
+            float m_steer;
+            float m_brake;
+            bool m_hand_brake;
+            bool m_reverse;
+            int32_t m_gear;
+            bool m_manual_gear_shift;
+
+        };
+    } // namespace msg
+} // namespace carla_msgs
+
+#endif // _FAST_DDS_GENERATED_CARLA_MSGS_MSG_CARLAEGOCarlaEgoVehicleControl_H_

LibCarla/source/carla/ros2/types/CarlaEgoVehicleControlPubSubTypes.cpp

@@ -0,0 +1,172 @@
+// Copyright 2016 Proyectos y Sistemas de Mantenimiento SL (eProsima).
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+/*!
+ * @file CarlaEgoCarlaEgoVehicleControlPubSubTypes.cpp
+ * This header file contains the implementation of the serialization functions.
+ *
+ * This file was generated by the tool fastcdrgen.
+ */
+
+#include <fastcdr/FastBuffer.h>
+#include <fastcdr/Cdr.h>
+
+#include "CarlaEgoVehicleControlPubSubTypes.h"
+
+using SerializedPayload_t = eprosima::fastrtps::rtps::SerializedPayload_t;
+using InstanceHandle_t = eprosima::fastrtps::rtps::InstanceHandle_t;
+
+namespace carla_msgs {
+    namespace msg {
+        CarlaEgoVehicleControlPubSubType::CarlaEgoVehicleControlPubSubType()
+        {
+            setName("carla_msgs::msg::dds_::CarlaEgoVehicleControl_");
+            auto type_size = CarlaEgoVehicleControl::getMaxCdrSerializedSize();
+            type_size += eprosima::fastcdr::Cdr::alignment(type_size, 4); /* possible submessage alignment */
+            m_typeSize = static_cast<uint32_t>(type_size) + 4; /*encapsulation*/
+            m_isGetKeyDefined = CarlaEgoVehicleControl::isKeyDefined();
+            size_t keyLength = CarlaEgoVehicleControl::getKeyMaxCdrSerializedSize() > 16 ?
+                    CarlaEgoVehicleControl::getKeyMaxCdrSerializedSize() : 16;
+            m_keyBuffer = reinterpret_cast<unsigned char*>(malloc(keyLength));
+            memset(m_keyBuffer, 0, keyLength);
+        }
+
+        CarlaEgoVehicleControlPubSubType::~CarlaEgoVehicleControlPubSubType()
+        {
+            if (m_keyBuffer != nullptr)
+            {
+                free(m_keyBuffer);
+            }
+        }
+
+        bool CarlaEgoVehicleControlPubSubType::serialize(
+                void* data,
+                SerializedPayload_t* payload)
+        {
+            CarlaEgoVehicleControl* p_type = static_cast<CarlaEgoVehicleControl*>(data);
+
+            // Object that manages the raw buffer.
+            eprosima::fastcdr::FastBuffer fastbuffer(reinterpret_cast<char*>(payload->data), payload->max_size);
+            // Object that serializes the data.
+            eprosima::fastcdr::Cdr ser(fastbuffer, eprosima::fastcdr::Cdr::DEFAULT_ENDIAN, eprosima::fastcdr::Cdr::DDS_CDR);
+            payload->encapsulation = ser.endianness() == eprosima::fastcdr::Cdr::BIG_ENDIANNESS ? CDR_BE : CDR_LE;
+            // Serialize encapsulation
+            ser.serialize_encapsulation();
+
+            try
+            {
+                // Serialize the object.
+                p_type->serialize(ser);
+            }
+            catch (eprosima::fastcdr::exception::NotEnoughMemoryException& /*exception*/)
+            {
+                return false;
+            }
+
+            // Get the serialized length
+            payload->length = static_cast<uint32_t>(ser.getSerializedDataLength());
+            return true;
+        }
+
+        bool CarlaEgoVehicleControlPubSubType::deserialize(
+                SerializedPayload_t* payload,
+                void* data)
+        {
+            try
+            {
+                //Convert DATA to pointer of your type
+                CarlaEgoVehicleControl* p_type = static_cast<CarlaEgoVehicleControl*>(data);
+
+                // Object that manages the raw buffer.
+                eprosima::fastcdr::FastBuffer fastbuffer(reinterpret_cast<char*>(payload->data), payload->length);
+
+                // Object that deserializes the data.
+                eprosima::fastcdr::Cdr deser(fastbuffer, eprosima::fastcdr::Cdr::DEFAULT_ENDIAN, eprosima::fastcdr::Cdr::DDS_CDR);
+
+                // Deserialize encapsulation.
+                deser.read_encapsulation();
+                payload->encapsulation = deser.endianness() == eprosima::fastcdr::Cdr::BIG_ENDIANNESS ? CDR_BE : CDR_LE;
+
+                // Deserialize the object.
+                p_type->deserialize(deser);
+            }
+            catch (eprosima::fastcdr::exception::NotEnoughMemoryException& /*exception*/)
+            {
+                return false;
+            }
+
+            return true;
+        }
+
+        std::function<uint32_t()> CarlaEgoVehicleControlPubSubType::getSerializedSizeProvider(
+                void* data)
+        {
+            return [data]() -> uint32_t
+                   {
+                       return static_cast<uint32_t>(type::getCdrSerializedSize(*static_cast<CarlaEgoVehicleControl*>(data))) +
+                              4u /*encapsulation*/;
+                   };
+        }
+
+        void* CarlaEgoVehicleControlPubSubType::createData()
+        {
+            return reinterpret_cast<void*>(new CarlaEgoVehicleControl());
+        }
+
+        void CarlaEgoVehicleControlPubSubType::deleteData(
+                void* data)
+        {
+            delete(reinterpret_cast<CarlaEgoVehicleControl*>(data));
+        }
+
+        bool CarlaEgoVehicleControlPubSubType::getKey(
+                void* data,
+                InstanceHandle_t* handle,
+                bool force_md5)
+        {
+            if (!m_isGetKeyDefined)
+            {
+                return false;
+            }
+
+            CarlaEgoVehicleControl* p_type = static_cast<CarlaEgoVehicleControl*>(data);
+
+            // Object that manages the raw buffer.
+            eprosima::fastcdr::FastBuffer fastbuffer(reinterpret_cast<char*>(m_keyBuffer),
+                    CarlaEgoVehicleControl::getKeyMaxCdrSerializedSize());
+
+            // Object that serializes the data.
+            eprosima::fastcdr::Cdr ser(fastbuffer, eprosima::fastcdr::Cdr::BIG_ENDIANNESS);
+            p_type->serializeKey(ser);
+            if (force_md5 || CarlaEgoVehicleControl::getKeyMaxCdrSerializedSize() > 16)
+            {
+                m_md5.init();
+                m_md5.update(m_keyBuffer, static_cast<unsigned int>(ser.getSerializedDataLength()));
+                m_md5.finalize();
+                for (uint8_t i = 0; i < 16; ++i)
+                {
+                    handle->value[i] = m_md5.digest[i];
+                }
+            }
+            else
+            {
+                for (uint8_t i = 0; i < 16; ++i)
+                {
+                    handle->value[i] = m_keyBuffer[i];
+                }
+            }
+            return true;
+        }
+    } //End of namespace msg
+} //End of namespace carla_msgs

LibCarla/source/carla/ros2/types/CarlaEgoVehicleControlPubSubTypes.h

@@ -0,0 +1,107 @@
+// Copyright 2016 Proyectos y Sistemas de Mantenimiento SL (eProsima).
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+/*!
+ * @file CarlaEgoCarlaEgoVehicleControlPubSubTypes.h
+ * This header file contains the declaration of the serialization functions.
+ *
+ * This file was generated by the tool fastcdrgen.
+ */
+
+#ifndef _FAST_DDS_GENERATED_CARLA_MSGS_MSG_CARLAEGOCarlaEgoVehicleControl_PUBSUBTYPES_H_
+#define _FAST_DDS_GENERATED_CARLA_MSGS_MSG_CARLAEGOCarlaEgoVehicleControl_PUBSUBTYPES_H_
+
+#include <fastdds/dds/topic/TopicDataType.hpp>
+#include <fastrtps/utils/md5.h>
+
+#include "CarlaEgoVehicleControl.h"
+
+#include "HeaderPubSubTypes.h"
+
+#if !defined(GEN_API_VER) || (GEN_API_VER != 1)
+#error \
+    Generated CarlaEgoCarlaEgoVehicleControl is not compatible with current installed Fast DDS. Please, regenerate it with fastddsgen.
+#endif  // GEN_API_VER
+
+namespace carla_msgs
+{
+    namespace msg
+    {
+        /*!
+         * @brief This class represents the TopicDataType of the type CarlaEgoVehicleControl defined by the user in the IDL file.
+         * @ingroup CarlaEgoVehicleControl
+         */
+        class CarlaEgoVehicleControlPubSubType : public eprosima::fastdds::dds::TopicDataType
+        {
+        public:
+
+            typedef CarlaEgoVehicleControl type;
+
+            eProsima_user_DllExport CarlaEgoVehicleControlPubSubType();
+
+            eProsima_user_DllExport virtual ~CarlaEgoVehicleControlPubSubType() override;
+
+            eProsima_user_DllExport virtual bool serialize(
+                    void* data,
+                    eprosima::fastrtps::rtps::SerializedPayload_t* payload) override;
+
+            eProsima_user_DllExport virtual bool deserialize(
+                    eprosima::fastrtps::rtps::SerializedPayload_t* payload,
+                    void* data) override;
+
+            eProsima_user_DllExport virtual std::function<uint32_t()> getSerializedSizeProvider(
+                    void* data) override;
+
+            eProsima_user_DllExport virtual bool getKey(
+                    void* data,
+                    eprosima::fastrtps::rtps::InstanceHandle_t* ihandle,
+                    bool force_md5 = false) override;
+
+            eProsima_user_DllExport virtual void* createData() override;
+
+            eProsima_user_DllExport virtual void deleteData(
+                    void* data) override;
+
+        #ifdef TOPIC_DATA_TYPE_API_HAS_IS_BOUNDED
+            eProsima_user_DllExport inline bool is_bounded() const override
+            {
+                return false;
+            }
+
+        #endif  // TOPIC_DATA_TYPE_API_HAS_IS_BOUNDED
+
+        #ifdef TOPIC_DATA_TYPE_API_HAS_IS_PLAIN
+            eProsima_user_DllExport inline bool is_plain() const override
+            {
+                return false;
+            }
+
+        #endif  // TOPIC_DATA_TYPE_API_HAS_IS_PLAIN
+
+        #ifdef TOPIC_DATA_TYPE_API_HAS_CONSTRUCT_SAMPLE
+            eProsima_user_DllExport inline bool construct_sample(
+                    void* memory) const override
+            {
+                (void)memory;
+                return false;
+            }
+
+        #endif  // TOPIC_DATA_TYPE_API_HAS_CONSTRUCT_SAMPLE
+            MD5 m_md5;
+            unsigned char* m_keyBuffer;
+        };
+    }
+}
+
+#endif // _FAST_DDS_GENERATED_CARLA_MSGS_MSG_CARLAEGOCarlaEgoVehicleControl_PUBSUBTYPES_H_

LibCarla/source/carla/ros2/types/CarlaLineInvasion.cpp

@@ -0,0 +1,225 @@
+// Copyright 2016 Proyectos y Sistemas de Mantenimiento SL (eProsima).
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+/*!
+ * @file CarlaLineInvasion.cpp
+ * This source file contains the definition of the described types in the IDL file.
+ *
+ * This file was generated by the tool gen.
+ */
+
+#ifdef _WIN32
+// Remove linker warning LNK4221 on Visual Studio
+namespace {
+char dummy;
+}  // namespace
+#endif  // _WIN32
+
+#include "CarlaLineInvasion.h"
+#include <fastcdr/Cdr.h>
+
+#include <fastcdr/exceptions/BadParamException.h>
+using namespace eprosima::fastcdr::exception;
+
+#include <utility>
+
+#define carla_msgs_msg_std_msgs_msg_Header_max_cdr_typesize 268ULL;
+#define carla_msgs_msg_LaneInvasionEvent_max_cdr_typesize 672ULL;
+#define carla_msgs_msg_std_msgs_msg_Time_max_cdr_typesize 8ULL;
+#define carla_msgs_msg_std_msgs_msg_Header_max_key_cdr_typesize 0ULL;
+#define carla_msgs_msg_LaneInvasionEvent_max_key_cdr_typesize 0ULL;
+#define carla_msgs_msg_std_msgs_msg_Time_max_key_cdr_typesize 0ULL;
+
+carla_msgs::msg::LaneInvasionEvent::LaneInvasionEvent()
+{
+}
+
+carla_msgs::msg::LaneInvasionEvent::~LaneInvasionEvent()
+{
+}
+
+carla_msgs::msg::LaneInvasionEvent::LaneInvasionEvent(
+        const LaneInvasionEvent& x)
+{
+    m_header = x.m_header;
+    m_crossed_lane_markings = x.m_crossed_lane_markings;
+}
+
+carla_msgs::msg::LaneInvasionEvent::LaneInvasionEvent(
+        LaneInvasionEvent&& x) noexcept
+{
+    m_header = std::move(x.m_header);
+    m_crossed_lane_markings = std::move(x.m_crossed_lane_markings);
+}
+
+carla_msgs::msg::LaneInvasionEvent& carla_msgs::msg::LaneInvasionEvent::operator =(
+        const LaneInvasionEvent& x)
+{
+    m_header = x.m_header;
+    m_crossed_lane_markings = x.m_crossed_lane_markings;
+
+    return *this;
+}
+
+carla_msgs::msg::LaneInvasionEvent& carla_msgs::msg::LaneInvasionEvent::operator =(
+        LaneInvasionEvent&& x) noexcept
+{
+    m_header = std::move(x.m_header);
+    m_crossed_lane_markings = std::move(x.m_crossed_lane_markings);
+
+    return *this;
+}
+
+bool carla_msgs::msg::LaneInvasionEvent::operator ==(
+        const LaneInvasionEvent& x) const
+{
+    return (m_header == x.m_header && m_crossed_lane_markings == x.m_crossed_lane_markings);
+}
+
+bool carla_msgs::msg::LaneInvasionEvent::operator !=(
+        const LaneInvasionEvent& x) const
+{
+    return !(*this == x);
+}
+
+size_t carla_msgs::msg::LaneInvasionEvent::getMaxCdrSerializedSize(
+        size_t current_alignment)
+{
+    static_cast<void>(current_alignment);
+    return carla_msgs_msg_LaneInvasionEvent_max_cdr_typesize;
+}
+
+size_t carla_msgs::msg::LaneInvasionEvent::getCdrSerializedSize(
+        const carla_msgs::msg::LaneInvasionEvent& data,
+        size_t current_alignment)
+{
+    size_t initial_alignment = current_alignment;
+    current_alignment += std_msgs::msg::Header::getCdrSerializedSize(data.header(), current_alignment);
+    current_alignment += 4 + eprosima::fastcdr::Cdr::alignment(current_alignment, 4);
+
+    if (data.crossed_lane_markings().size() > 0)
+    {
+        current_alignment += (data.crossed_lane_markings().size() * 4) + eprosima::fastcdr::Cdr::alignment(current_alignment, 4);
+    }
+
+    return current_alignment - initial_alignment;
+}
+
+void carla_msgs::msg::LaneInvasionEvent::serialize(
+        eprosima::fastcdr::Cdr& scdr) const
+{
+    scdr << m_header;
+    scdr << m_crossed_lane_markings;
+}
+
+void carla_msgs::msg::LaneInvasionEvent::deserialize(
+        eprosima::fastcdr::Cdr& dcdr)
+{
+    dcdr >> m_header;
+    dcdr >> m_crossed_lane_markings;
+}
+
+/*!
+ * @brief This function copies the value in member header
+ * @param _header New value to be copied in member header
+ */
+void carla_msgs::msg::LaneInvasionEvent::header(
+        const std_msgs::msg::Header& _header)
+{
+    m_header = _header;
+}
+
+/*!
+ * @brief This function moves the value in member header
+ * @param _header New value to be moved in member header
+ */
+void carla_msgs::msg::LaneInvasionEvent::header(
+        std_msgs::msg::Header&& _header)
+{
+    m_header = std::move(_header);
+}
+
+/*!
+ * @brief This function returns a constant reference to member header
+ * @return Constant reference to member header
+ */
+const std_msgs::msg::Header& carla_msgs::msg::LaneInvasionEvent::header() const
+{
+    return m_header;
+}
+
+/*!
+ * @brief This function returns a reference to member header
+ * @return Reference to member header
+ */
+std_msgs::msg::Header& carla_msgs::msg::LaneInvasionEvent::header()
+{
+    return m_header;
+}
+/*!
+ * @brief This function copies the value in member crossed_lane_markings
+ * @param _crossed_lane_markings New value to be copied in member crossed_lane_markings
+ */
+void carla_msgs::msg::LaneInvasionEvent::crossed_lane_markings(
+        const std::vector<int32_t>& _crossed_lane_markings)
+{
+    m_crossed_lane_markings = _crossed_lane_markings;
+}
+
+/*!
+ * @brief This function moves the value in member crossed_lane_markings
+ * @param _crossed_lane_markings New value to be moved in member crossed_lane_markings
+ */
+void carla_msgs::msg::LaneInvasionEvent::crossed_lane_markings(
+        std::vector<int32_t>&& _crossed_lane_markings)
+{
+    m_crossed_lane_markings = std::move(_crossed_lane_markings);
+}
+
+/*!
+ * @brief This function returns a constant reference to member crossed_lane_markings
+ * @return Constant reference to member crossed_lane_markings
+ */
+const std::vector<int32_t>& carla_msgs::msg::LaneInvasionEvent::crossed_lane_markings() const
+{
+    return m_crossed_lane_markings;
+}
+
+/*!
+ * @brief This function returns a reference to member crossed_lane_markings
+ * @return Reference to member crossed_lane_markings
+ */
+std::vector<int32_t>& carla_msgs::msg::LaneInvasionEvent::crossed_lane_markings()
+{
+    return m_crossed_lane_markings;
+}
+
+
+size_t carla_msgs::msg::LaneInvasionEvent::getKeyMaxCdrSerializedSize(
+        size_t current_alignment)
+{
+    static_cast<void>(current_alignment);
+    return carla_msgs_msg_LaneInvasionEvent_max_key_cdr_typesize;
+}
+
+bool carla_msgs::msg::LaneInvasionEvent::isKeyDefined()
+{
+    return false;
+}
+
+void carla_msgs::msg::LaneInvasionEvent::serializeKey(
+        eprosima::fastcdr::Cdr& scdr) const
+{
+    (void) scdr;
+}

LibCarla/source/carla/ros2/types/CarlaLineInvasion.h

@@ -0,0 +1,242 @@
+// Copyright 2016 Proyectos y Sistemas de Mantenimiento SL (eProsima).
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+/*!
+ * @file CarlaLineInvasion.h
+ * This header file contains the declaration of the described types in the IDL file.
+ *
+ * This file was generated by the tool gen.
+ */
+
+#ifndef _FAST_DDS_GENERATED_CARLA_MSGS_MSG_CARLALINEINVASION_H_
+#define _FAST_DDS_GENERATED_CARLA_MSGS_MSG_CARLALINEINVASION_H_
+
+#include "Header.h"
+
+#include <fastrtps/utils/fixed_size_string.hpp>
+
+#include <stdint.h>
+#include <array>
+#include <string>
+#include <vector>
+#include <map>
+#include <bitset>
+
+#if defined(_WIN32)
+#if defined(EPROSIMA_USER_DLL_EXPORT)
+#define eProsima_user_DllExport __declspec( dllexport )
+#else
+#define eProsima_user_DllExport
+#endif  // EPROSIMA_USER_DLL_EXPORT
+#else
+#define eProsima_user_DllExport
+#endif  // _WIN32
+
+#if defined(_WIN32)
+#if defined(EPROSIMA_USER_DLL_EXPORT)
+#if defined(CarlaLineInvasion_SOURCE)
+#define CarlaLineInvasion_DllAPI __declspec( dllexport )
+#else
+#define CarlaLineInvasion_DllAPI __declspec( dllimport )
+#endif // CarlaLineInvasion_SOURCE
+#else
+#define CarlaLineInvasion_DllAPI
+#endif  // EPROSIMA_USER_DLL_EXPORT
+#else
+#define CarlaLineInvasion_DllAPI
+#endif // _WIN32
+
+namespace eprosima {
+namespace fastcdr {
+class Cdr;
+} // namespace fastcdr
+} // namespace eprosima
+
+namespace carla_msgs {
+    namespace msg {
+        const int32_t LANE_MARKING_OTHER = 0;
+        const int32_t LANE_MARKING_BROKEN = 1;
+        const int32_t LANE_MARKING_SOLID = 2;
+        /*!
+         * @brief This class represents the structure LaneInvasionEvent defined by the user in the IDL file.
+         * @ingroup CARLALINEINVASION
+         */
+        class LaneInvasionEvent
+        {
+        public:
+            /*!
+             * @brief Default constructor.
+             */
+            eProsima_user_DllExport LaneInvasionEvent();
+
+            /*!
+             * @brief Default destructor.
+             */
+            eProsima_user_DllExport ~LaneInvasionEvent();
+
+            /*!
+             * @brief Copy constructor.
+             * @param x Reference to the object carla_msgs::msg::LaneInvasionEvent that will be copied.
+             */
+            eProsima_user_DllExport LaneInvasionEvent(
+                    const LaneInvasionEvent& x);
+
+            /*!
+             * @brief Move constructor.
+             * @param x Reference to the object carla_msgs::msg::LaneInvasionEvent that will be copied.
+             */
+            eProsima_user_DllExport LaneInvasionEvent(
+                    LaneInvasionEvent&& x) noexcept;
+
+            /*!
+             * @brief Copy assignment.
+             * @param x Reference to the object carla_msgs::msg::LaneInvasionEvent that will be copied.
+             */
+            eProsima_user_DllExport LaneInvasionEvent& operator =(
+                    const LaneInvasionEvent& x);
+
+            /*!
+             * @brief Move assignment.
+             * @param x Reference to the object carla_msgs::msg::LaneInvasionEvent that will be copied.
+             */
+            eProsima_user_DllExport LaneInvasionEvent& operator =(
+                    LaneInvasionEvent&& x) noexcept;
+
+            /*!
+             * @brief Comparison operator.
+             * @param x carla_msgs::msg::LaneInvasionEvent object to compare.
+             */
+            eProsima_user_DllExport bool operator ==(
+                    const LaneInvasionEvent& x) const;
+
+            /*!
+             * @brief Comparison operator.
+             * @param x carla_msgs::msg::LaneInvasionEvent object to compare.
+             */
+            eProsima_user_DllExport bool operator !=(
+                    const LaneInvasionEvent& x) const;
+
+            /*!
+             * @brief This function copies the value in member header
+             * @param _header New value to be copied in member header
+             */
+            eProsima_user_DllExport void header(
+                    const std_msgs::msg::Header& _header);
+
+            /*!
+             * @brief This function moves the value in member header
+             * @param _header New value to be moved in member header
+             */
+            eProsima_user_DllExport void header(
+                    std_msgs::msg::Header&& _header);
+
+            /*!
+             * @brief This function returns a constant reference to member header
+             * @return Constant reference to member header
+             */
+            eProsima_user_DllExport const std_msgs::msg::Header& header() const;
+
+            /*!
+             * @brief This function returns a reference to member header
+             * @return Reference to member header
+             */
+            eProsima_user_DllExport std_msgs::msg::Header& header();
+            /*!
+             * @brief This function copies the value in member crossed_lane_markings
+             * @param _crossed_lane_markings New value to be copied in member crossed_lane_markings
+             */
+            eProsima_user_DllExport void crossed_lane_markings(
+                    const std::vector<int32_t>& _crossed_lane_markings);
+
+            /*!
+             * @brief This function moves the value in member crossed_lane_markings
+             * @param _crossed_lane_markings New value to be moved in member crossed_lane_markings
+             */
+            eProsima_user_DllExport void crossed_lane_markings(
+                    std::vector<int32_t>&& _crossed_lane_markings);
+
+            /*!
+             * @brief This function returns a constant reference to member crossed_lane_markings
+             * @return Constant reference to member crossed_lane_markings
+             */
+            eProsima_user_DllExport const std::vector<int32_t>& crossed_lane_markings() const;
+
+            /*!
+             * @brief This function returns a reference to member crossed_lane_markings
+             * @return Reference to member crossed_lane_markings
+             */
+            eProsima_user_DllExport std::vector<int32_t>& crossed_lane_markings();
+
+            /*!
+            * @brief This function returns the maximum serialized size of an object
+            * depending on the buffer alignment.
+            * @param current_alignment Buffer alignment.
+            * @return Maximum serialized size.
+            */
+            eProsima_user_DllExport static size_t getMaxCdrSerializedSize(
+                    size_t current_alignment = 0);
+
+            /*!
+             * @brief This function returns the serialized size of a data depending on the buffer alignment.
+             * @param data Data which is calculated its serialized size.
+             * @param current_alignment Buffer alignment.
+             * @return Serialized size.
+             */
+            eProsima_user_DllExport static size_t getCdrSerializedSize(
+                    const carla_msgs::msg::LaneInvasionEvent& data,
+                    size_t current_alignment = 0);
+
+            /*!
+             * @brief This function serializes an object using CDR serialization.
+             * @param cdr CDR serialization object.
+             */
+            eProsima_user_DllExport void serialize(
+                    eprosima::fastcdr::Cdr& cdr) const;
+
+            /*!
+             * @brief This function deserializes an object using CDR serialization.
+             * @param cdr CDR serialization object.
+             */
+            eProsima_user_DllExport void deserialize(
+                    eprosima::fastcdr::Cdr& cdr);
+
+            /*!
+             * @brief This function returns the maximum serialized size of the Key of an object
+             * depending on the buffer alignment.
+             * @param current_alignment Buffer alignment.
+             * @return Maximum serialized size.
+             */
+            eProsima_user_DllExport static size_t getKeyMaxCdrSerializedSize(
+                    size_t current_alignment = 0);
+
+            /*!
+             * @brief This function tells you if the Key has been defined for this type
+             */
+            eProsima_user_DllExport static bool isKeyDefined();
+
+            /*!
+             * @brief This function serializes the key members of an object using CDR serialization.
+             * @param cdr CDR serialization object.
+             */
+            eProsima_user_DllExport void serializeKey(
+                    eprosima::fastcdr::Cdr& cdr) const;
+
+        private:
+            std_msgs::msg::Header m_header;
+            std::vector<int32_t> m_crossed_lane_markings;
+        };
+    } // namespace msg
+} // namespace carla_msgs
+
+#endif // _FAST_DDS_GENERATED_CARLA_MSGS_MSG_CARLALINEINVASION_H_

LibCarla/source/carla/ros2/types/CarlaLineInvasionPubSubTypes.cpp

@@ -0,0 +1,172 @@
+// Copyright 2016 Proyectos y Sistemas de Mantenimiento SL (eProsima).
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+/*!
+ * @file CarlaLineInvasionPubSubTypes.cpp
+ * This header file contains the implementation of the serialization functions.
+ *
+ * This file was generated by the tool fastcdrgen.
+ */
+
+#include <fastcdr/FastBuffer.h>
+#include <fastcdr/Cdr.h>
+
+#include "CarlaLineInvasionPubSubTypes.h"
+
+using SerializedPayload_t = eprosima::fastrtps::rtps::SerializedPayload_t;
+using InstanceHandle_t = eprosima::fastrtps::rtps::InstanceHandle_t;
+
+namespace carla_msgs {
+    namespace msg {
+        LaneInvasionEventPubSubType::LaneInvasionEventPubSubType()
+        {
+            setName("carla_msgs::msg::dds_::LaneInvasionEvent_");
+            auto type_size = LaneInvasionEvent::getMaxCdrSerializedSize();
+            type_size += eprosima::fastcdr::Cdr::alignment(type_size, 4); /* possible submessage alignment */
+            m_typeSize = static_cast<uint32_t>(type_size) + 4; /*encapsulation*/
+            m_isGetKeyDefined = LaneInvasionEvent::isKeyDefined();
+            size_t keyLength = LaneInvasionEvent::getKeyMaxCdrSerializedSize() > 16 ?
+                    LaneInvasionEvent::getKeyMaxCdrSerializedSize() : 16;
+            m_keyBuffer = reinterpret_cast<unsigned char*>(malloc(keyLength));
+            memset(m_keyBuffer, 0, keyLength);
+        }
+
+        LaneInvasionEventPubSubType::~LaneInvasionEventPubSubType()
+        {
+            if (m_keyBuffer != nullptr)
+            {
+                free(m_keyBuffer);
+            }
+        }
+
+        bool LaneInvasionEventPubSubType::serialize(
+                void* data,
+                SerializedPayload_t* payload)
+        {
+            LaneInvasionEvent* p_type = static_cast<LaneInvasionEvent*>(data);
+
+            // Object that manages the raw buffer.
+            eprosima::fastcdr::FastBuffer fastbuffer(reinterpret_cast<char*>(payload->data), payload->max_size);
+            // Object that serializes the data.
+            eprosima::fastcdr::Cdr ser(fastbuffer, eprosima::fastcdr::Cdr::DEFAULT_ENDIAN, eprosima::fastcdr::Cdr::DDS_CDR);
+            payload->encapsulation = ser.endianness() == eprosima::fastcdr::Cdr::BIG_ENDIANNESS ? CDR_BE : CDR_LE;
+            // Serialize encapsulation
+            ser.serialize_encapsulation();
+
+            try
+            {
+                // Serialize the object.
+                p_type->serialize(ser);
+            }
+            catch (eprosima::fastcdr::exception::NotEnoughMemoryException& /*exception*/)
+            {
+                return false;
+            }
+
+            // Get the serialized length
+            payload->length = static_cast<uint32_t>(ser.getSerializedDataLength());
+            return true;
+        }
+
+        bool LaneInvasionEventPubSubType::deserialize(
+                SerializedPayload_t* payload,
+                void* data)
+        {
+            try
+            {
+                //Convert DATA to pointer of your type
+                LaneInvasionEvent* p_type = static_cast<LaneInvasionEvent*>(data);
+
+                // Object that manages the raw buffer.
+                eprosima::fastcdr::FastBuffer fastbuffer(reinterpret_cast<char*>(payload->data), payload->length);
+
+                // Object that deserializes the data.
+                eprosima::fastcdr::Cdr deser(fastbuffer, eprosima::fastcdr::Cdr::DEFAULT_ENDIAN, eprosima::fastcdr::Cdr::DDS_CDR);
+
+                // Deserialize encapsulation.
+                deser.read_encapsulation();
+                payload->encapsulation = deser.endianness() == eprosima::fastcdr::Cdr::BIG_ENDIANNESS ? CDR_BE : CDR_LE;
+
+                // Deserialize the object.
+                p_type->deserialize(deser);
+            }
+            catch (eprosima::fastcdr::exception::NotEnoughMemoryException& /*exception*/)
+            {
+                return false;
+            }
+
+            return true;
+        }
+
+        std::function<uint32_t()> LaneInvasionEventPubSubType::getSerializedSizeProvider(
+                void* data)
+        {
+            return [data]() -> uint32_t
+                   {
+                       return static_cast<uint32_t>(type::getCdrSerializedSize(*static_cast<LaneInvasionEvent*>(data))) +
+                              4u /*encapsulation*/;
+                   };
+        }
+
+        void* LaneInvasionEventPubSubType::createData()
+        {
+            return reinterpret_cast<void*>(new LaneInvasionEvent());
+        }
+
+        void LaneInvasionEventPubSubType::deleteData(
+                void* data)
+        {
+            delete(reinterpret_cast<LaneInvasionEvent*>(data));
+        }
+
+        bool LaneInvasionEventPubSubType::getKey(
+                void* data,
+                InstanceHandle_t* handle,
+                bool force_md5)
+        {
+            if (!m_isGetKeyDefined)
+            {
+                return false;
+            }
+
+            LaneInvasionEvent* p_type = static_cast<LaneInvasionEvent*>(data);
+
+            // Object that manages the raw buffer.
+            eprosima::fastcdr::FastBuffer fastbuffer(reinterpret_cast<char*>(m_keyBuffer),
+                    LaneInvasionEvent::getKeyMaxCdrSerializedSize());
+
+            // Object that serializes the data.
+            eprosima::fastcdr::Cdr ser(fastbuffer, eprosima::fastcdr::Cdr::BIG_ENDIANNESS);
+            p_type->serializeKey(ser);
+            if (force_md5 || LaneInvasionEvent::getKeyMaxCdrSerializedSize() > 16)
+            {
+                m_md5.init();
+                m_md5.update(m_keyBuffer, static_cast<unsigned int>(ser.getSerializedDataLength()));
+                m_md5.finalize();
+                for (uint8_t i = 0; i < 16; ++i)
+                {
+                    handle->value[i] = m_md5.digest[i];
+                }
+            }
+            else
+            {
+                for (uint8_t i = 0; i < 16; ++i)
+                {
+                    handle->value[i] = m_keyBuffer[i];
+                }
+            }
+            return true;
+        }
+    } //End of namespace msg
+} //End of namespace carla_msgs

LibCarla/source/carla/ros2/types/CarlaLineInvasionPubSubTypes.h

@@ -0,0 +1,105 @@
+// Copyright 2016 Proyectos y Sistemas de Mantenimiento SL (eProsima).
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+/*!
+ * @file CarlaLineInvasionPubSubTypes.h
+ * This header file contains the declaration of the serialization functions.
+ *
+ * This file was generated by the tool fastcdrgen.
+ */
+
+#ifndef _FAST_DDS_GENERATED_CARLA_MSGS_MSG_CARLALINEINVASION_PUBSUBTYPES_H_
+#define _FAST_DDS_GENERATED_CARLA_MSGS_MSG_CARLALINEINVASION_PUBSUBTYPES_H_
+
+#include <fastdds/dds/topic/TopicDataType.hpp>
+#include <fastrtps/utils/md5.h>
+
+#include "CarlaLineInvasion.h"
+#include "HeaderPubSubTypes.h"
+
+#if !defined(GEN_API_VER) || (GEN_API_VER != 1)
+#error \
+    Generated CarlaLineInvasion is not compatible with current installed Fast DDS. Please, regenerate it with fastddsgen.
+#endif  // GEN_API_VER
+
+namespace carla_msgs
+{
+    namespace msg
+    {
+        /*!
+         * @brief This class represents the TopicDataType of the type LaneInvasionEvent defined by the user in the IDL file.
+         * @ingroup CARLALINEINVASION
+         */
+        class LaneInvasionEventPubSubType : public eprosima::fastdds::dds::TopicDataType
+        {
+        public:
+            typedef LaneInvasionEvent type;
+
+            eProsima_user_DllExport LaneInvasionEventPubSubType();
+
+            eProsima_user_DllExport virtual ~LaneInvasionEventPubSubType() override;
+
+            eProsima_user_DllExport virtual bool serialize(
+                    void* data,
+                    eprosima::fastrtps::rtps::SerializedPayload_t* payload) override;
+
+            eProsima_user_DllExport virtual bool deserialize(
+                    eprosima::fastrtps::rtps::SerializedPayload_t* payload,
+                    void* data) override;
+
+            eProsima_user_DllExport virtual std::function<uint32_t()> getSerializedSizeProvider(
+                    void* data) override;
+
+            eProsima_user_DllExport virtual bool getKey(
+                    void* data,
+                    eprosima::fastrtps::rtps::InstanceHandle_t* ihandle,
+                    bool force_md5 = false) override;
+
+            eProsima_user_DllExport virtual void* createData() override;
+
+            eProsima_user_DllExport virtual void deleteData(
+                    void* data) override;
+
+        #ifdef TOPIC_DATA_TYPE_API_HAS_IS_BOUNDED
+            eProsima_user_DllExport inline bool is_bounded() const override
+            {
+                return false;
+            }
+
+        #endif  // TOPIC_DATA_TYPE_API_HAS_IS_BOUNDED
+
+        #ifdef TOPIC_DATA_TYPE_API_HAS_IS_PLAIN
+            eProsima_user_DllExport inline bool is_plain() const override
+            {
+                return false;
+            }
+
+        #endif  // TOPIC_DATA_TYPE_API_HAS_IS_PLAIN
+
+        #ifdef TOPIC_DATA_TYPE_API_HAS_CONSTRUCT_SAMPLE
+            eProsima_user_DllExport inline bool construct_sample(
+                    void* memory) const override
+            {
+                (void)memory;
+                return false;
+            }
+
+        #endif  // TOPIC_DATA_TYPE_API_HAS_CONSTRUCT_SAMPLE
+            MD5 m_md5;
+            unsigned char* m_keyBuffer;
+        };
+    }
+}
+
+#endif // _FAST_DDS_GENERATED_CARLA_MSGS_MSG_CARLALINEINVASION_PUBSUBTYPES_H_

LibCarla/source/carla/ros2/types/Float32.cpp

@@ -0,0 +1,160 @@
+// Copyright 2016 Proyectos y Sistemas de Mantenimiento SL (eProsima).
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+/*!
+ * @file Float32.cpp
+ * This source file contains the definition of the described types in the IDL file.
+ *
+ * This file was generated by the tool gen.
+ */
+
+#ifdef _WIN32
+// Remove linker warning LNK4221 on Visual Studio
+namespace {
+char dummy;
+}  // namespace
+#endif  // _WIN32
+
+#include "Float32.h"
+#include <fastcdr/Cdr.h>
+
+#include <fastcdr/exceptions/BadParamException.h>
+using namespace eprosima::fastcdr::exception;
+
+#include <utility>
+
+#define std_msgs_msg_Float32_max_cdr_typesize 4ULL;
+#define std_msgs_msg_Float32_max_key_cdr_typesize 0ULL;
+
+std_msgs::msg::Float32::Float32()
+{
+    m_data = 0.0;
+}
+
+std_msgs::msg::Float32::~Float32()
+{
+}
+
+std_msgs::msg::Float32::Float32(
+        const Float32& x)
+{
+    m_data = x.m_data;
+}
+
+std_msgs::msg::Float32::Float32(
+        Float32&& x) noexcept
+{
+    m_data = x.m_data;
+}
+
+std_msgs::msg::Float32& std_msgs::msg::Float32::operator =(
+        const Float32& x)
+{
+    m_data = x.m_data;
+    return *this;
+}
+
+std_msgs::msg::Float32& std_msgs::msg::Float32::operator =(
+        Float32&& x) noexcept
+{
+    m_data = x.m_data;
+    return *this;
+}
+
+bool std_msgs::msg::Float32::operator ==(
+        const Float32& x) const
+{
+    return (m_data == x.m_data);
+}
+
+bool std_msgs::msg::Float32::operator !=(
+        const Float32& x) const
+{
+    return !(*this == x);
+}
+
+size_t std_msgs::msg::Float32::getMaxCdrSerializedSize(
+        size_t current_alignment)
+{
+    static_cast<void>(current_alignment);
+    return std_msgs_msg_Float32_max_cdr_typesize;
+}
+
+size_t std_msgs::msg::Float32::getCdrSerializedSize(
+        const std_msgs::msg::Float32& data,
+        size_t current_alignment)
+{
+    (void)data;
+    size_t initial_alignment = current_alignment;
+    current_alignment += 4 + eprosima::fastcdr::Cdr::alignment(current_alignment, 4);
+    return current_alignment - initial_alignment;
+}
+
+void std_msgs::msg::Float32::serialize(
+        eprosima::fastcdr::Cdr& scdr) const
+{
+    scdr << m_data;
+}
+
+void std_msgs::msg::Float32::deserialize(
+        eprosima::fastcdr::Cdr& dcdr)
+{
+    dcdr >> m_data;
+}
+
+/*!
+ * @brief This function sets a value in member data
+ * @param _data New value for member data
+ */
+void std_msgs::msg::Float32::data(
+        float _data)
+{
+    m_data = _data;
+}
+
+/*!
+ * @brief This function returns the value of member data
+ * @return Value of member data
+ */
+float std_msgs::msg::Float32::data() const
+{
+    return m_data;
+}
+
+/*!
+ * @brief This function returns a reference to member data
+ * @return Reference to member data
+ */
+float& std_msgs::msg::Float32::data()
+{
+    return m_data;
+}
+
+size_t std_msgs::msg::Float32::getKeyMaxCdrSerializedSize(
+        size_t current_alignment)
+{
+    static_cast<void>(current_alignment);
+    return std_msgs_msg_Float32_max_key_cdr_typesize;
+}
+
+bool std_msgs::msg::Float32::isKeyDefined()
+{
+    return false;
+}
+
+void std_msgs::msg::Float32::serializeKey(
+        eprosima::fastcdr::Cdr& scdr) const
+{
+    (void) scdr;
+}

LibCarla/source/carla/ros2/types/Float32.h

@@ -0,0 +1,204 @@
+// Copyright 2016 Proyectos y Sistemas de Mantenimiento SL (eProsima).
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+/*!
+ * @file Float32.h
+ * This header file contains the declaration of the described types in the IDL file.
+ *
+ * This file was generated by the tool gen.
+ */
+
+#ifndef _FAST_DDS_GENERATED_STD_MSGS_MSG_FLOAT32_H_
+#define _FAST_DDS_GENERATED_STD_MSGS_MSG_FLOAT32_H_
+
+#include <fastrtps/utils/fixed_size_string.hpp>
+
+#include <stdint.h>
+#include <array>
+#include <string>
+#include <vector>
+#include <map>
+#include <bitset>
+
+#if defined(_WIN32)
+#if defined(EPROSIMA_USER_DLL_EXPORT)
+#define eProsima_user_DllExport __declspec( dllexport )
+#else
+#define eProsima_user_DllExport
+#endif  // EPROSIMA_USER_DLL_EXPORT
+#else
+#define eProsima_user_DllExport
+#endif  // _WIN32
+
+#if defined(_WIN32)
+#if defined(EPROSIMA_USER_DLL_EXPORT)
+#if defined(Float32_SOURCE)
+#define Float32_DllAPI __declspec( dllexport )
+#else
+#define Float32_DllAPI __declspec( dllimport )
+#endif // Float32_SOURCE
+#else
+#define Float32_DllAPI
+#endif  // EPROSIMA_USER_DLL_EXPORT
+#else
+#define Float32_DllAPI
+#endif // _WIN32
+
+namespace eprosima {
+namespace fastcdr {
+class Cdr;
+} // namespace fastcdr
+} // namespace eprosima
+
+namespace std_msgs {
+    namespace msg {
+        /*!
+         * @brief This class represents the structure Float32 defined by the user in the IDL file.
+         * @ingroup FLOAT32
+         */
+        class Float32
+        {
+        public:
+            /*!
+             * @brief Default constructor.
+             */
+            eProsima_user_DllExport Float32();
+
+            /*!
+             * @brief Default destructor.
+             */
+            eProsima_user_DllExport ~Float32();
+
+            /*!
+             * @brief Copy constructor.
+             * @param x Reference to the object std_msgs::msg::Float32 that will be copied.
+             */
+            eProsima_user_DllExport Float32(
+                    const Float32& x);
+
+            /*!
+             * @brief Move constructor.
+             * @param x Reference to the object std_msgs::msg::Float32 that will be copied.
+             */
+            eProsima_user_DllExport Float32(
+                    Float32&& x) noexcept;
+
+            /*!
+             * @brief Copy assignment.
+             * @param x Reference to the object std_msgs::msg::Float32 that will be copied.
+             */
+            eProsima_user_DllExport Float32& operator =(
+                    const Float32& x);
+
+            /*!
+             * @brief Move assignment.
+             * @param x Reference to the object std_msgs::msg::Float32 that will be copied.
+             */
+            eProsima_user_DllExport Float32& operator =(
+                    Float32&& x) noexcept;
+
+            /*!
+             * @brief Comparison operator.
+             * @param x std_msgs::msg::Float32 object to compare.
+             */
+            eProsima_user_DllExport bool operator ==(
+                    const Float32& x) const;
+
+            /*!
+             * @brief Comparison operator.
+             * @param x std_msgs::msg::Float32 object to compare.
+             */
+            eProsima_user_DllExport bool operator !=(
+                    const Float32& x) const;
+
+            /*!
+             * @brief This function sets a value in member data
+             * @param _data New value for member data
+             */
+            eProsima_user_DllExport void data(
+                    float _data);
+
+            /*!
+             * @brief This function returns the value of member data
+             * @return Value of member data
+             */
+            eProsima_user_DllExport float data() const;
+
+            /*!
+             * @brief This function returns a reference to member data
+             * @return Reference to member data
+             */
+            eProsima_user_DllExport float& data();
+
+            /*!
+            * @brief This function returns the maximum serialized size of an object
+            * depending on the buffer alignment.
+            * @param current_alignment Buffer alignment.
+            * @return Maximum serialized size.
+            */
+            eProsima_user_DllExport static size_t getMaxCdrSerializedSize(
+                    size_t current_alignment = 0);
+
+            /*!
+             * @brief This function returns the serialized size of a data depending on the buffer alignment.
+             * @param data Data which is calculated its serialized size.
+             * @param current_alignment Buffer alignment.
+             * @return Serialized size.
+             */
+            eProsima_user_DllExport static size_t getCdrSerializedSize(
+                    const std_msgs::msg::Float32& data,
+                    size_t current_alignment = 0);
+
+            /*!
+             * @brief This function serializes an object using CDR serialization.
+             * @param cdr CDR serialization object.
+             */
+            eProsima_user_DllExport void serialize(
+                    eprosima::fastcdr::Cdr& cdr) const;
+
+            /*!
+             * @brief This function deserializes an object using CDR serialization.
+             * @param cdr CDR serialization object.
+             */
+            eProsima_user_DllExport void deserialize(
+                    eprosima::fastcdr::Cdr& cdr);
+
+            /*!
+             * @brief This function returns the maximum serialized size of the Key of an object
+             * depending on the buffer alignment.
+             * @param current_alignment Buffer alignment.
+             * @return Maximum serialized size.
+             */
+            eProsima_user_DllExport static size_t getKeyMaxCdrSerializedSize(
+                    size_t current_alignment = 0);
+
+            /*!
+             * @brief This function tells you if the Key has been defined for this type
+             */
+            eProsima_user_DllExport static bool isKeyDefined();
+
+            /*!
+             * @brief This function serializes the key members of an object using CDR serialization.
+             * @param cdr CDR serialization object.
+             */
+            eProsima_user_DllExport void serializeKey(
+                    eprosima::fastcdr::Cdr& cdr) const;
+
+        private:
+            float m_data;
+        };
+    } // namespace msg
+} // namespace std_msgs
+
+#endif // _FAST_DDS_GENERATED_STD_MSGS_MSG_FLOAT32_H_

LibCarla/source/carla/ros2/types/Float32PubSubTypes.cpp

@@ -0,0 +1,172 @@
+// Copyright 2016 Proyectos y Sistemas de Mantenimiento SL (eProsima).
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+/*!
+ * @file Float32PubSubTypes.cpp
+ * This header file contains the implementation of the serialization functions.
+ *
+ * This file was generated by the tool fastcdrgen.
+ */
+
+#include <fastcdr/FastBuffer.h>
+#include <fastcdr/Cdr.h>
+
+#include "Float32PubSubTypes.h"
+
+using SerializedPayload_t = eprosima::fastrtps::rtps::SerializedPayload_t;
+using InstanceHandle_t = eprosima::fastrtps::rtps::InstanceHandle_t;
+
+namespace std_msgs {
+    namespace msg {
+        Float32PubSubType::Float32PubSubType()
+        {
+            setName("std_msgs::msg::dds_::Float32_");
+            auto type_size = Float32::getMaxCdrSerializedSize();
+            type_size += eprosima::fastcdr::Cdr::alignment(type_size, 4); /* possible submessage alignment */
+            m_typeSize = static_cast<uint32_t>(type_size) + 4; /*encapsulation*/
+            m_isGetKeyDefined = Float32::isKeyDefined();
+            size_t keyLength = Float32::getKeyMaxCdrSerializedSize() > 16 ?
+                    Float32::getKeyMaxCdrSerializedSize() : 16;
+            m_keyBuffer = reinterpret_cast<unsigned char*>(malloc(keyLength));
+            memset(m_keyBuffer, 0, keyLength);
+        }
+
+        Float32PubSubType::~Float32PubSubType()
+        {
+            if (m_keyBuffer != nullptr)
+            {
+                free(m_keyBuffer);
+            }
+        }
+
+        bool Float32PubSubType::serialize(
+                void* data,
+                SerializedPayload_t* payload)
+        {
+            Float32* p_type = static_cast<Float32*>(data);
+
+            // Object that manages the raw buffer.
+            eprosima::fastcdr::FastBuffer fastbuffer(reinterpret_cast<char*>(payload->data), payload->max_size);
+            // Object that serializes the data.
+            eprosima::fastcdr::Cdr ser(fastbuffer, eprosima::fastcdr::Cdr::DEFAULT_ENDIAN, eprosima::fastcdr::Cdr::DDS_CDR);
+            payload->encapsulation = ser.endianness() == eprosima::fastcdr::Cdr::BIG_ENDIANNESS ? CDR_BE : CDR_LE;
+            // Serialize encapsulation
+            ser.serialize_encapsulation();
+
+            try
+            {
+                // Serialize the object.
+                p_type->serialize(ser);
+            }
+            catch (eprosima::fastcdr::exception::NotEnoughMemoryException& /*exception*/)
+            {
+                return false;
+            }
+
+            // Get the serialized length
+            payload->length = static_cast<uint32_t>(ser.getSerializedDataLength());
+            return true;
+        }
+
+        bool Float32PubSubType::deserialize(
+                SerializedPayload_t* payload,
+                void* data)
+        {
+            try
+            {
+                //Convert DATA to pointer of your type
+                Float32* p_type = static_cast<Float32*>(data);
+
+                // Object that manages the raw buffer.
+                eprosima::fastcdr::FastBuffer fastbuffer(reinterpret_cast<char*>(payload->data), payload->length);
+
+                // Object that deserializes the data.
+                eprosima::fastcdr::Cdr deser(fastbuffer, eprosima::fastcdr::Cdr::DEFAULT_ENDIAN, eprosima::fastcdr::Cdr::DDS_CDR);
+
+                // Deserialize encapsulation.
+                deser.read_encapsulation();
+                payload->encapsulation = deser.endianness() == eprosima::fastcdr::Cdr::BIG_ENDIANNESS ? CDR_BE : CDR_LE;
+
+                // Deserialize the object.
+                p_type->deserialize(deser);
+            }
+            catch (eprosima::fastcdr::exception::NotEnoughMemoryException& /*exception*/)
+            {
+                return false;
+            }
+
+            return true;
+        }
+
+        std::function<uint32_t()> Float32PubSubType::getSerializedSizeProvider(
+                void* data)
+        {
+            return [data]() -> uint32_t
+                   {
+                       return static_cast<uint32_t>(type::getCdrSerializedSize(*static_cast<Float32*>(data))) +
+                              4u /*encapsulation*/;
+                   };
+        }
+
+        void* Float32PubSubType::createData()
+        {
+            return reinterpret_cast<void*>(new Float32());
+        }
+
+        void Float32PubSubType::deleteData(
+                void* data)
+        {
+            delete(reinterpret_cast<Float32*>(data));
+        }
+
+        bool Float32PubSubType::getKey(
+                void* data,
+                InstanceHandle_t* handle,
+                bool force_md5)
+        {
+            if (!m_isGetKeyDefined)
+            {
+                return false;
+            }
+
+            Float32* p_type = static_cast<Float32*>(data);
+
+            // Object that manages the raw buffer.
+            eprosima::fastcdr::FastBuffer fastbuffer(reinterpret_cast<char*>(m_keyBuffer),
+                    Float32::getKeyMaxCdrSerializedSize());
+
+            // Object that serializes the data.
+            eprosima::fastcdr::Cdr ser(fastbuffer, eprosima::fastcdr::Cdr::BIG_ENDIANNESS);
+            p_type->serializeKey(ser);
+            if (force_md5 || Float32::getKeyMaxCdrSerializedSize() > 16)
+            {
+                m_md5.init();
+                m_md5.update(m_keyBuffer, static_cast<unsigned int>(ser.getSerializedDataLength()));
+                m_md5.finalize();
+                for (uint8_t i = 0; i < 16; ++i)
+                {
+                    handle->value[i] = m_md5.digest[i];
+                }
+            }
+            else
+            {
+                for (uint8_t i = 0; i < 16; ++i)
+                {
+                    handle->value[i] = m_keyBuffer[i];
+                }
+            }
+            return true;
+        }
+    } //End of namespace msg
+} //End of namespace std_msgs

LibCarla/source/carla/ros2/types/Float32PubSubTypes.h

@@ -0,0 +1,139 @@
+// Copyright 2016 Proyectos y Sistemas de Mantenimiento SL (eProsima).
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+/*!
+ * @file Float32PubSubTypes.h
+ * This header file contains the declaration of the serialization functions.
+ *
+ * This file was generated by the tool fastcdrgen.
+ */
+
+#ifndef _FAST_DDS_GENERATED_STD_MSGS_MSG_FLOAT32_PUBSUBTYPES_H_
+#define _FAST_DDS_GENERATED_STD_MSGS_MSG_FLOAT32_PUBSUBTYPES_H_
+
+#include <fastdds/dds/topic/TopicDataType.hpp>
+#include <fastrtps/utils/md5.h>
+
+#include "Float32.h"
+
+#if !defined(GEN_API_VER) || (GEN_API_VER != 1)
+#error \
+    Generated Float32 is not compatible with current installed Fast DDS. Please, regenerate it with fastddsgen.
+#endif  // GEN_API_VER
+
+namespace std_msgs
+{
+    namespace msg
+    {
+        #ifndef SWIG
+        namespace detail {
+            template<typename Tag, typename Tag::type M>
+            struct Float32_rob
+            {
+                friend constexpr typename Tag::type get(
+                        Tag)
+                {
+                    return M;
+                }
+            };
+
+            struct Float32_f
+            {
+                typedef float Float32::* type;
+                friend constexpr type get(
+                        Float32_f);
+            };
+
+            template struct Float32_rob<Float32_f, &Float32::m_data>;
+
+            template <typename T, typename Tag>
+            inline size_t constexpr Float32_offset_of() {
+                return ((::size_t) &reinterpret_cast<char const volatile&>((((T*)0)->*get(Tag()))));
+            }
+        }
+        #endif
+
+        /*!
+         * @brief This class represents the TopicDataType of the type Float32 defined by the user in the IDL file.
+         * @ingroup FLOAT32
+         */
+        class Float32PubSubType : public eprosima::fastdds::dds::TopicDataType
+        {
+        public:
+
+            typedef Float32 type;
+
+            eProsima_user_DllExport Float32PubSubType();
+
+            eProsima_user_DllExport virtual ~Float32PubSubType() override;
+
+            eProsima_user_DllExport virtual bool serialize(
+                    void* data,
+                    eprosima::fastrtps::rtps::SerializedPayload_t* payload) override;
+
+            eProsima_user_DllExport virtual bool deserialize(
+                    eprosima::fastrtps::rtps::SerializedPayload_t* payload,
+                    void* data) override;
+
+            eProsima_user_DllExport virtual std::function<uint32_t()> getSerializedSizeProvider(
+                    void* data) override;
+
+            eProsima_user_DllExport virtual bool getKey(
+                    void* data,
+                    eprosima::fastrtps::rtps::InstanceHandle_t* ihandle,
+                    bool force_md5 = false) override;
+
+            eProsima_user_DllExport virtual void* createData() override;
+
+            eProsima_user_DllExport virtual void deleteData(
+                    void* data) override;
+
+        #ifdef TOPIC_DATA_TYPE_API_HAS_IS_BOUNDED
+            eProsima_user_DllExport inline bool is_bounded() const override
+            {
+                return true;
+            }
+
+        #endif  // TOPIC_DATA_TYPE_API_HAS_IS_BOUNDED
+
+        #ifdef TOPIC_DATA_TYPE_API_HAS_IS_PLAIN
+            eProsima_user_DllExport inline bool is_plain() const override
+            {
+                return is_plain_impl();
+            }
+
+        #endif  // TOPIC_DATA_TYPE_API_HAS_IS_PLAIN
+
+        #ifdef TOPIC_DATA_TYPE_API_HAS_CONSTRUCT_SAMPLE
+            eProsima_user_DllExport inline bool construct_sample(
+                    void* memory) const override
+            {
+                new (memory) Float32();
+                return true;
+            }
+
+        #endif  // TOPIC_DATA_TYPE_API_HAS_CONSTRUCT_SAMPLE
+            MD5 m_md5;
+            unsigned char* m_keyBuffer;
+
+        private:
+            static constexpr bool is_plain_impl()
+            {
+                return 4ULL == (detail::Float32_offset_of<Float32, detail::Float32_f>() + sizeof(float));
+
+            }};
+    }
+}
+
+#endif // _FAST_DDS_GENERATED_STD_MSGS_MSG_FLOAT32_PUBSUBTYPES_H_

LibCarla/source/carla/ros2/types/Header.cpp

@@ -0,0 +1,219 @@
+// Copyright 2016 Proyectos y Sistemas de Mantenimiento SL (eProsima).
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+/*!
+ * @file Header.cpp
+ * This source file contains the definition of the described types in the IDL file.
+ *
+ * This file was generated by the tool gen.
+ */
+
+#ifdef _WIN32
+// Remove linker warning LNK4221 on Visual Studio
+namespace {
+char dummy;
+}  // namespace
+#endif  // _WIN32
+
+#include "Header.h"
+#include <fastcdr/Cdr.h>
+
+#include <fastcdr/exceptions/BadParamException.h>
+using namespace eprosima::fastcdr::exception;
+
+#include <utility>
+
+#define builtin_interfaces_msg_Time_max_cdr_typesize 8ULL;
+#define std_msgs_msg_Header_max_cdr_typesize 268ULL;
+#define builtin_interfaces_msg_Time_max_key_cdr_typesize 0ULL;
+#define std_msgs_msg_Header_max_key_cdr_typesize 0ULL;
+
+std_msgs::msg::Header::Header()
+{
+    m_frame_id ="";
+}
+
+std_msgs::msg::Header::~Header()
+{
+}
+
+std_msgs::msg::Header::Header(
+        const Header& x)
+{
+    m_stamp = x.m_stamp;
+    m_frame_id = x.m_frame_id;
+}
+
+std_msgs::msg::Header::Header(
+        Header&& x) noexcept
+{
+    m_stamp = std::move(x.m_stamp);
+    m_frame_id = std::move(x.m_frame_id);
+}
+
+std_msgs::msg::Header& std_msgs::msg::Header::operator =(
+        const Header& x)
+{
+    m_stamp = x.m_stamp;
+    m_frame_id = x.m_frame_id;
+
+    return *this;
+}
+
+std_msgs::msg::Header& std_msgs::msg::Header::operator =(
+        Header&& x) noexcept
+{
+    m_stamp = std::move(x.m_stamp);
+    m_frame_id = std::move(x.m_frame_id);
+
+    return *this;
+}
+
+bool std_msgs::msg::Header::operator ==(
+        const Header& x) const
+{
+    return (m_stamp == x.m_stamp && m_frame_id == x.m_frame_id);
+}
+
+bool std_msgs::msg::Header::operator !=(
+        const Header& x) const
+{
+    return !(*this == x);
+}
+
+size_t std_msgs::msg::Header::getMaxCdrSerializedSize(
+        size_t current_alignment)
+{
+    static_cast<void>(current_alignment);
+    return std_msgs_msg_Header_max_cdr_typesize;
+}
+
+size_t std_msgs::msg::Header::getCdrSerializedSize(
+        const std_msgs::msg::Header& data,
+        size_t current_alignment)
+{
+    size_t initial_alignment = current_alignment;
+    current_alignment += builtin_interfaces::msg::Time::getCdrSerializedSize(data.stamp(), current_alignment);
+    current_alignment += 4 + eprosima::fastcdr::Cdr::alignment(current_alignment, 4) + data.frame_id().size() + 1;
+
+    return current_alignment - initial_alignment;
+}
+
+void std_msgs::msg::Header::serialize(
+        eprosima::fastcdr::Cdr& scdr) const
+{
+    scdr << m_stamp;
+    scdr << m_frame_id.c_str();
+}
+
+void std_msgs::msg::Header::deserialize(
+        eprosima::fastcdr::Cdr& dcdr)
+{
+    dcdr >> m_stamp;
+    dcdr >> m_frame_id;
+}
+
+/*!
+ * @brief This function copies the value in member stamp
+ * @param _stamp New value to be copied in member stamp
+ */
+void std_msgs::msg::Header::stamp(
+        const builtin_interfaces::msg::Time& _stamp)
+{
+    m_stamp = _stamp;
+}
+
+/*!
+ * @brief This function moves the value in member stamp
+ * @param _stamp New value to be moved in member stamp
+ */
+void std_msgs::msg::Header::stamp(
+        builtin_interfaces::msg::Time&& _stamp)
+{
+    m_stamp = std::move(_stamp);
+}
+
+/*!
+ * @brief This function returns a constant reference to member stamp
+ * @return Constant reference to member stamp
+ */
+const builtin_interfaces::msg::Time& std_msgs::msg::Header::stamp() const
+{
+    return m_stamp;
+}
+
+/*!
+ * @brief This function returns a reference to member stamp
+ * @return Reference to member stamp
+ */
+builtin_interfaces::msg::Time& std_msgs::msg::Header::stamp()
+{
+    return m_stamp;
+}
+/*!
+ * @brief This function copies the value in member frame_id
+ * @param _frame_id New value to be copied in member frame_id
+ */
+void std_msgs::msg::Header::frame_id(
+        const std::string& _frame_id)
+{
+    m_frame_id = _frame_id;
+}
+
+/*!
+ * @brief This function moves the value in member frame_id
+ * @param _frame_id New value to be moved in member frame_id
+ */
+void std_msgs::msg::Header::frame_id(
+        std::string&& _frame_id)
+{
+    m_frame_id = std::move(_frame_id);
+}
+
+/*!
+ * @brief This function returns a constant reference to member frame_id
+ * @return Constant reference to member frame_id
+ */
+const std::string& std_msgs::msg::Header::frame_id() const
+{
+    return m_frame_id;
+}
+
+/*!
+ * @brief This function returns a reference to member frame_id
+ * @return Reference to member frame_id
+ */
+std::string& std_msgs::msg::Header::frame_id()
+{
+    return m_frame_id;
+}
+
+
+size_t std_msgs::msg::Header::getKeyMaxCdrSerializedSize(
+        size_t current_alignment)
+{
+    static_cast<void>(current_alignment);
+    return std_msgs_msg_Header_max_key_cdr_typesize;
+}
+
+bool std_msgs::msg::Header::isKeyDefined()
+{
+    return false;
+}
+
+void std_msgs::msg::Header::serializeKey(
+        eprosima::fastcdr::Cdr& scdr) const
+{
+    (void) scdr;
+}

LibCarla/source/carla/ros2/types/Header.h

@@ -0,0 +1,240 @@
+// Copyright 2016 Proyectos y Sistemas de Mantenimiento SL (eProsima).
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+/*!
+ * @file Header.h
+ * This header file contains the declaration of the described types in the IDL file.
+ *
+ * This file was generated by the tool gen.
+ */
+
+#ifndef _FAST_DDS_GENERATED_STD_MSGS_MSG_HEADER_H_
+#define _FAST_DDS_GENERATED_STD_MSGS_MSG_HEADER_H_
+
+#include "Time.h"
+
+#include <fastrtps/utils/fixed_size_string.hpp>
+
+#include <stdint.h>
+#include <array>
+#include <string>
+#include <vector>
+#include <map>
+#include <bitset>
+
+#if defined(_WIN32)
+#if defined(EPROSIMA_USER_DLL_EXPORT)
+#define eProsima_user_DllExport __declspec( dllexport )
+#else
+#define eProsima_user_DllExport
+#endif  // EPROSIMA_USER_DLL_EXPORT
+#else
+#define eProsima_user_DllExport
+#endif  // _WIN32
+
+#if defined(_WIN32)
+#if defined(EPROSIMA_USER_DLL_EXPORT)
+#if defined(Header_SOURCE)
+#define Header_DllAPI __declspec( dllexport )
+#else
+#define Header_DllAPI __declspec( dllimport )
+#endif // Header_SOURCE
+#else
+#define Header_DllAPI
+#endif  // EPROSIMA_USER_DLL_EXPORT
+#else
+#define Header_DllAPI
+#endif // _WIN32
+
+namespace eprosima {
+namespace fastcdr {
+class Cdr;
+} // namespace fastcdr
+} // namespace eprosima
+
+namespace std_msgs {
+    namespace msg {
+        /*!
+         * @brief This class represents the structure Header defined by the user in the IDL file.
+         * @ingroup HEADER
+         */
+        class Header
+        {
+        public:
+
+            /*!
+             * @brief Default constructor.
+             */
+            eProsima_user_DllExport Header();
+
+            /*!
+             * @brief Default destructor.
+             */
+            eProsima_user_DllExport ~Header();
+
+            /*!
+             * @brief Copy constructor.
+             * @param x Reference to the object std_msgs::msg::Header that will be copied.
+             */
+            eProsima_user_DllExport Header(
+                    const Header& x);
+
+            /*!
+             * @brief Move constructor.
+             * @param x Reference to the object std_msgs::msg::Header that will be copied.
+             */
+            eProsima_user_DllExport Header(
+                    Header&& x) noexcept;
+
+            /*!
+             * @brief Copy assignment.
+             * @param x Reference to the object std_msgs::msg::Header that will be copied.
+             */
+            eProsima_user_DllExport Header& operator =(
+                    const Header& x);
+
+            /*!
+             * @brief Move assignment.
+             * @param x Reference to the object std_msgs::msg::Header that will be copied.
+             */
+            eProsima_user_DllExport Header& operator =(
+                    Header&& x) noexcept;
+
+            /*!
+             * @brief Comparison operator.
+             * @param x std_msgs::msg::Header object to compare.
+             */
+            eProsima_user_DllExport bool operator ==(
+                    const Header& x) const;
+
+            /*!
+             * @brief Comparison operator.
+             * @param x std_msgs::msg::Header object to compare.
+             */
+            eProsima_user_DllExport bool operator !=(
+                    const Header& x) const;
+
+            /*!
+             * @brief This function copies the value in member stamp
+             * @param _stamp New value to be copied in member stamp
+             */
+            eProsima_user_DllExport void stamp(
+                    const builtin_interfaces::msg::Time& _stamp);
+
+            /*!
+             * @brief This function moves the value in member stamp
+             * @param _stamp New value to be moved in member stamp
+             */
+            eProsima_user_DllExport void stamp(
+                    builtin_interfaces::msg::Time&& _stamp);
+
+            /*!
+             * @brief This function returns a constant reference to member stamp
+             * @return Constant reference to member stamp
+             */
+            eProsima_user_DllExport const builtin_interfaces::msg::Time& stamp() const;
+
+            /*!
+             * @brief This function returns a reference to member stamp
+             * @return Reference to member stamp
+             */
+            eProsima_user_DllExport builtin_interfaces::msg::Time& stamp();
+            /*!
+             * @brief This function copies the value in member frame_id
+             * @param _frame_id New value to be copied in member frame_id
+             */
+            eProsima_user_DllExport void frame_id(
+                    const std::string& _frame_id);
+
+            /*!
+             * @brief This function moves the value in member frame_id
+             * @param _frame_id New value to be moved in member frame_id
+             */
+            eProsima_user_DllExport void frame_id(
+                    std::string&& _frame_id);
+
+            /*!
+             * @brief This function returns a constant reference to member frame_id
+             * @return Constant reference to member frame_id
+             */
+            eProsima_user_DllExport const std::string& frame_id() const;
+
+            /*!
+             * @brief This function returns a reference to member frame_id
+             * @return Reference to member frame_id
+             */
+            eProsima_user_DllExport std::string& frame_id();
+
+            /*!
+            * @brief This function returns the maximum serialized size of an object
+            * depending on the buffer alignment.
+            * @param current_alignment Buffer alignment.
+            * @return Maximum serialized size.
+            */
+            eProsima_user_DllExport static size_t getMaxCdrSerializedSize(
+                    size_t current_alignment = 0);
+
+            /*!
+             * @brief This function returns the serialized size of a data depending on the buffer alignment.
+             * @param data Data which is calculated its serialized size.
+             * @param current_alignment Buffer alignment.
+             * @return Serialized size.
+             */
+            eProsima_user_DllExport static size_t getCdrSerializedSize(
+                    const std_msgs::msg::Header& data,
+                    size_t current_alignment = 0);
+
+            /*!
+             * @brief This function serializes an object using CDR serialization.
+             * @param cdr CDR serialization object.
+             */
+            eProsima_user_DllExport void serialize(
+                    eprosima::fastcdr::Cdr& cdr) const;
+
+            /*!
+             * @brief This function deserializes an object using CDR serialization.
+             * @param cdr CDR serialization object.
+             */
+            eProsima_user_DllExport void deserialize(
+                    eprosima::fastcdr::Cdr& cdr);
+
+            /*!
+             * @brief This function returns the maximum serialized size of the Key of an object
+             * depending on the buffer alignment.
+             * @param current_alignment Buffer alignment.
+             * @return Maximum serialized size.
+             */
+            eProsima_user_DllExport static size_t getKeyMaxCdrSerializedSize(
+                    size_t current_alignment = 0);
+
+            /*!
+             * @brief This function tells you if the Key has been defined for this type
+             */
+            eProsima_user_DllExport static bool isKeyDefined();
+
+            /*!
+             * @brief This function serializes the key members of an object using CDR serialization.
+             * @param cdr CDR serialization object.
+             */
+            eProsima_user_DllExport void serializeKey(
+                    eprosima::fastcdr::Cdr& cdr) const;
+
+        private:
+            builtin_interfaces::msg::Time m_stamp;
+            std::string m_frame_id;
+        };
+    } // namespace msg
+} // namespace std_msgs
+
+#endif // _FAST_DDS_GENERATED_STD_MSGS_MSG_HEADER_H_

LibCarla/source/carla/ros2/types/HeaderPubSubTypes.cpp

@@ -0,0 +1,154 @@
+// Copyright 2016 Proyectos y Sistemas de Mantenimiento SL (eProsima).
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+/*!
+ * @file HeaderPubSubTypes.cpp
+ * This header file contains the implementation of the serialization functions.
+ *
+ * This file was generated by the tool fastcdrgen.
+ */
+
+#include <fastcdr/FastBuffer.h>
+#include <fastcdr/Cdr.h>
+
+#include "HeaderPubSubTypes.h"
+
+using SerializedPayload_t = eprosima::fastrtps::rtps::SerializedPayload_t;
+using InstanceHandle_t = eprosima::fastrtps::rtps::InstanceHandle_t;
+
+namespace std_msgs {
+    namespace msg {
+        HeaderPubSubType::HeaderPubSubType()
+        {
+            setName("std_msgs::msg::dds_::Header_");
+            auto type_size = Header::getMaxCdrSerializedSize();
+            type_size += eprosima::fastcdr::Cdr::alignment(type_size, 4); /* possible submessage alignment */
+            m_typeSize = static_cast<uint32_t>(type_size) + 4; /*encapsulation*/
+            m_isGetKeyDefined = Header::isKeyDefined();
+            size_t keyLength = Header::getKeyMaxCdrSerializedSize() > 16 ?
+                    Header::getKeyMaxCdrSerializedSize() : 16;
+            m_keyBuffer = reinterpret_cast<unsigned char*>(malloc(keyLength));
+            memset(m_keyBuffer, 0, keyLength);
+        }
+
+        HeaderPubSubType::~HeaderPubSubType()
+        {
+            if (m_keyBuffer != nullptr)
+            {
+                free(m_keyBuffer);
+            }
+        }
+
+        bool HeaderPubSubType::serialize(
+                void* data,
+                SerializedPayload_t* payload)
+        {
+            Header* p_type = static_cast<Header*>(data);
+
+            // Object that manages the raw buffer.
+            eprosima::fastcdr::FastBuffer fastbuffer(reinterpret_cast<char*>(payload->data), payload->max_size);
+            // Object that serializes the data.
+            eprosima::fastcdr::Cdr ser(fastbuffer, eprosima::fastcdr::Cdr::DEFAULT_ENDIAN, eprosima::fastcdr::Cdr::DDS_CDR);
+            payload->encapsulation = ser.endianness() == eprosima::fastcdr::Cdr::BIG_ENDIANNESS ? CDR_BE : CDR_LE;
+            // Serialize encapsulation
+            ser.serialize_encapsulation();
+            p_type->serialize(ser);
+            // Get the serialized length
+            payload->length = static_cast<uint32_t>(ser.getSerializedDataLength());
+            return true;
+        }
+
+        bool HeaderPubSubType::deserialize(
+                SerializedPayload_t* payload,
+                void* data)
+        {
+            //Convert DATA to pointer of your type
+            Header* p_type = static_cast<Header*>(data);
+
+            // Object that manages the raw buffer.
+            eprosima::fastcdr::FastBuffer fastbuffer(reinterpret_cast<char*>(payload->data), payload->length);
+
+            // Object that deserializes the data.
+            eprosima::fastcdr::Cdr deser(fastbuffer, eprosima::fastcdr::Cdr::DEFAULT_ENDIAN, eprosima::fastcdr::Cdr::DDS_CDR);
+
+            // Deserialize encapsulation.
+            deser.read_encapsulation();
+            payload->encapsulation = deser.endianness() == eprosima::fastcdr::Cdr::BIG_ENDIANNESS ? CDR_BE : CDR_LE;
+
+            // Deserialize the object.
+            p_type->deserialize(deser);
+            return true;
+        }
+
+        std::function<uint32_t()> HeaderPubSubType::getSerializedSizeProvider(
+                void* data)
+        {
+            return [data]() -> uint32_t
+                   {
+                       return static_cast<uint32_t>(type::getCdrSerializedSize(*static_cast<Header*>(data))) +
+                              4u /*encapsulation*/;
+                   };
+        }
+
+        void* HeaderPubSubType::createData()
+        {
+            return reinterpret_cast<void*>(new Header());
+        }
+
+        void HeaderPubSubType::deleteData(
+                void* data)
+        {
+            delete(reinterpret_cast<Header*>(data));
+        }
+
+        bool HeaderPubSubType::getKey(
+                void* data,
+                InstanceHandle_t* handle,
+                bool force_md5)
+        {
+            if (!m_isGetKeyDefined)
+            {
+                return false;
+            }
+
+            Header* p_type = static_cast<Header*>(data);
+
+            // Object that manages the raw buffer.
+            eprosima::fastcdr::FastBuffer fastbuffer(reinterpret_cast<char*>(m_keyBuffer),
+                    Header::getKeyMaxCdrSerializedSize());
+
+            // Object that serializes the data.
+            eprosima::fastcdr::Cdr ser(fastbuffer, eprosima::fastcdr::Cdr::BIG_ENDIANNESS);
+            p_type->serializeKey(ser);
+            if (force_md5 || Header::getKeyMaxCdrSerializedSize() > 16)
+            {
+                m_md5.init();
+                m_md5.update(m_keyBuffer, static_cast<unsigned int>(ser.getSerializedDataLength()));
+                m_md5.finalize();
+                for (uint8_t i = 0; i < 16; ++i)
+                {
+                    handle->value[i] = m_md5.digest[i];
+                }
+            }
+            else
+            {
+                for (uint8_t i = 0; i < 16; ++i)
+                {
+                    handle->value[i] = m_keyBuffer[i];
+                }
+            }
+            return true;
+        }
+    } //End of namespace msg
+} //End of namespace std_msgs

LibCarla/source/carla/ros2/types/HeaderPubSubTypes.h

@@ -0,0 +1,106 @@
+// Copyright 2016 Proyectos y Sistemas de Mantenimiento SL (eProsima).
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+/*!
+ * @file HeaderPubSubTypes.h
+ * This header file contains the declaration of the serialization functions.
+ *
+ * This file was generated by the tool fastcdrgen.
+ */
+
+#ifndef _FAST_DDS_GENERATED_STD_MSGS_MSG_HEADER_PUBSUBTYPES_H_
+#define _FAST_DDS_GENERATED_STD_MSGS_MSG_HEADER_PUBSUBTYPES_H_
+
+#include <fastdds/dds/topic/TopicDataType.hpp>
+#include <fastrtps/utils/md5.h>
+
+#include "Header.h"
+
+#if !defined(GEN_API_VER) || (GEN_API_VER != 1)
+#error \
+    Generated Header is not compatible with current installed Fast DDS. Please, regenerate it with fastddsgen.
+#endif  // GEN_API_VER
+
+namespace std_msgs
+{
+    namespace msg
+    {
+        /*!
+         * @brief This class represents the TopicDataType of the type Header defined by the user in the IDL file.
+         * @ingroup HEADER
+         */
+        class HeaderPubSubType : public eprosima::fastdds::dds::TopicDataType
+        {
+        public:
+
+            typedef Header type;
+
+            eProsima_user_DllExport HeaderPubSubType();
+
+            eProsima_user_DllExport virtual ~HeaderPubSubType() override;
+
+            eProsima_user_DllExport virtual bool serialize(
+                    void* data,
+                    eprosima::fastrtps::rtps::SerializedPayload_t* payload) override;
+
+            eProsima_user_DllExport virtual bool deserialize(
+                    eprosima::fastrtps::rtps::SerializedPayload_t* payload,
+                    void* data) override;
+
+            eProsima_user_DllExport virtual std::function<uint32_t()> getSerializedSizeProvider(
+                    void* data) override;
+
+            eProsima_user_DllExport virtual bool getKey(
+                    void* data,
+                    eprosima::fastrtps::rtps::InstanceHandle_t* ihandle,
+                    bool force_md5 = false) override;
+
+            eProsima_user_DllExport virtual void* createData() override;
+
+            eProsima_user_DllExport virtual void deleteData(
+                    void* data) override;
+
+        #ifdef TOPIC_DATA_TYPE_API_HAS_IS_BOUNDED
+            eProsima_user_DllExport inline bool is_bounded() const override
+            {
+                return false;
+            }
+
+        #endif  // TOPIC_DATA_TYPE_API_HAS_IS_BOUNDED
+
+        #ifdef TOPIC_DATA_TYPE_API_HAS_IS_PLAIN
+            eProsima_user_DllExport inline bool is_plain() const override
+            {
+                return false;
+            }
+
+        #endif  // TOPIC_DATA_TYPE_API_HAS_IS_PLAIN
+
+        #ifdef TOPIC_DATA_TYPE_API_HAS_CONSTRUCT_SAMPLE
+            eProsima_user_DllExport inline bool construct_sample(
+                    void* memory) const override
+            {
+                (void)memory;
+                return false;
+            }
+
+        #endif  // TOPIC_DATA_TYPE_API_HAS_CONSTRUCT_SAMPLE
+
+            MD5 m_md5;
+            unsigned char* m_keyBuffer;
+        };
+    }
+}
+
+#endif // _FAST_DDS_GENERATED_STD_MSGS_MSG_HEADER_PUBSUBTYPES_H_

LibCarla/source/carla/ros2/types/Image.cpp

@@ -0,0 +1,423 @@
+// Copyright 2016 Proyectos y Sistemas de Mantenimiento SL (eProsima).
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+/*!
+ * @file Image.cpp
+ * This source file contains the definition of the described types in the IDL file.
+ *
+ * This file was generated by the tool gen.
+ */
+
+#ifdef _WIN32
+// Remove linker warning LNK4221 on Visual Studio
+namespace {
+char dummy;
+}  // namespace
+#endif  // _WIN32
+
+#include "Image.h"
+#include <fastcdr/Cdr.h>
+
+#include <fastcdr/exceptions/BadParamException.h>
+using namespace eprosima::fastcdr::exception;
+
+#include <utility>
+
+#define builtin_interfaces_msg_Time_max_cdr_typesize 8ULL;
+#define sensor_msgs_msg_Image_max_cdr_typesize 648ULL;
+#define std_msgs_msg_Header_max_cdr_typesize 268ULL;
+#define builtin_interfaces_msg_Time_max_key_cdr_typesize 0ULL;
+#define sensor_msgs_msg_Image_max_key_cdr_typesize 0ULL;
+#define std_msgs_msg_Header_max_key_cdr_typesize 0ULL;
+
+sensor_msgs::msg::Image::Image()
+{
+    // std_msgs::msg::Header m_header
+
+    // unsigned long m_height
+    m_height = 0;
+    // unsigned long m_width
+    m_width = 0;
+    // string m_encoding
+    m_encoding ="";
+    // uint8 m_is_bigendian
+    m_is_bigendian = 0;
+    // unsigned long m_step
+    m_step = 0;
+    // sequence<uint8> m_data
+}
+
+sensor_msgs::msg::Image::~Image()
+{
+}
+
+sensor_msgs::msg::Image::Image(
+        const Image& x)
+{
+    m_header = x.m_header;
+    m_height = x.m_height;
+    m_width = x.m_width;
+    m_encoding = x.m_encoding;
+    m_is_bigendian = x.m_is_bigendian;
+    m_step = x.m_step;
+    m_data = x.m_data;
+}
+
+sensor_msgs::msg::Image::Image(
+        Image&& x) noexcept
+{
+    m_header = std::move(x.m_header);
+    m_height = x.m_height;
+    m_width = x.m_width;
+    m_encoding = std::move(x.m_encoding);
+    m_is_bigendian = x.m_is_bigendian;
+    m_step = x.m_step;
+    m_data = std::move(x.m_data);
+}
+
+sensor_msgs::msg::Image& sensor_msgs::msg::Image::operator =(
+        const Image& x)
+{
+    m_header = x.m_header;
+    m_height = x.m_height;
+    m_width = x.m_width;
+    m_encoding = x.m_encoding;
+    m_is_bigendian = x.m_is_bigendian;
+    m_step = x.m_step;
+    m_data = x.m_data;
+
+    return *this;
+}
+
+sensor_msgs::msg::Image& sensor_msgs::msg::Image::operator =(
+        Image&& x) noexcept
+{
+    m_header = std::move(x.m_header);
+    m_height = x.m_height;
+    m_width = x.m_width;
+    m_encoding = std::move(x.m_encoding);
+    m_is_bigendian = x.m_is_bigendian;
+    m_step = x.m_step;
+    m_data = std::move(x.m_data);
+
+    return *this;
+}
+
+bool sensor_msgs::msg::Image::operator ==(
+        const Image& x) const
+{
+    return (m_header == x.m_header && m_height == x.m_height && m_width == x.m_width && m_encoding == x.m_encoding && m_is_bigendian == x.m_is_bigendian && m_step == x.m_step && m_data == x.m_data);
+}
+
+bool sensor_msgs::msg::Image::operator !=(
+        const Image& x) const
+{
+    return !(*this == x);
+}
+
+size_t sensor_msgs::msg::Image::getMaxCdrSerializedSize(
+        size_t current_alignment)
+{
+    static_cast<void>(current_alignment);
+    return sensor_msgs_msg_Image_max_cdr_typesize;
+}
+
+size_t sensor_msgs::msg::Image::getCdrSerializedSize(
+        const sensor_msgs::msg::Image& data,
+        size_t current_alignment)
+{
+    size_t initial_alignment = current_alignment;
+    current_alignment += std_msgs::msg::Header::getCdrSerializedSize(data.header(), current_alignment);
+    current_alignment += 4 + eprosima::fastcdr::Cdr::alignment(current_alignment, 4);
+    current_alignment += 4 + eprosima::fastcdr::Cdr::alignment(current_alignment, 4);
+    current_alignment += 4 + eprosima::fastcdr::Cdr::alignment(current_alignment, 4) + data.encoding().size() + 1;
+    current_alignment += 1 + eprosima::fastcdr::Cdr::alignment(current_alignment, 1);
+    current_alignment += 4 + eprosima::fastcdr::Cdr::alignment(current_alignment, 4);
+    current_alignment += 4 + eprosima::fastcdr::Cdr::alignment(current_alignment, 4);
+
+    if (data.data().size() > 0)
+    {
+        current_alignment += (data.data().size() * 1) + eprosima::fastcdr::Cdr::alignment(current_alignment, 1);
+    }
+
+    return current_alignment - initial_alignment;
+}
+
+void sensor_msgs::msg::Image::serialize(
+        eprosima::fastcdr::Cdr& scdr) const
+{
+    scdr << m_header;
+    scdr << m_height;
+    scdr << m_width;
+    scdr << m_encoding.c_str();
+    scdr << m_is_bigendian;
+    scdr << m_step;
+    scdr << m_data;
+}
+
+void sensor_msgs::msg::Image::deserialize(
+        eprosima::fastcdr::Cdr& dcdr)
+{
+    dcdr >> m_header;
+    dcdr >> m_height;
+    dcdr >> m_width;
+    dcdr >> m_encoding;
+    dcdr >> m_is_bigendian;
+    dcdr >> m_step;
+    dcdr >> m_data;
+}
+
+/*!
+ * @brief This function copies the value in member header
+ * @param _header New value to be copied in member header
+ */
+void sensor_msgs::msg::Image::header(
+        const std_msgs::msg::Header& _header)
+{
+    m_header = _header;
+}
+
+/*!
+ * @brief This function moves the value in member header
+ * @param _header New value to be moved in member header
+ */
+void sensor_msgs::msg::Image::header(
+        std_msgs::msg::Header&& _header)
+{
+    m_header = std::move(_header);
+}
+
+/*!
+ * @brief This function returns a constant reference to member header
+ * @return Constant reference to member header
+ */
+const std_msgs::msg::Header& sensor_msgs::msg::Image::header() const
+{
+    return m_header;
+}
+
+/*!
+ * @brief This function returns a reference to member header
+ * @return Reference to member header
+ */
+std_msgs::msg::Header& sensor_msgs::msg::Image::header()
+{
+    return m_header;
+}
+
+/*!
+ * @brief This function sets a value in member height
+ * @param _height New value for member height
+ */
+void sensor_msgs::msg::Image::height(
+        uint32_t _height)
+{
+    m_height = _height;
+}
+
+/*!
+ * @brief This function returns the value of member height
+ * @return Value of member height
+ */
+uint32_t sensor_msgs::msg::Image::height() const
+{
+    return m_height;
+}
+
+/*!
+ * @brief This function returns a reference to member height
+ * @return Reference to member height
+ */
+uint32_t& sensor_msgs::msg::Image::height()
+{
+    return m_height;
+}
+
+/*!
+ * @brief This function sets a value in member width
+ * @param _width New value for member width
+ */
+void sensor_msgs::msg::Image::width(
+        uint32_t _width)
+{
+    m_width = _width;
+}
+
+/*!
+ * @brief This function returns the value of member width
+ * @return Value of member width
+ */
+uint32_t sensor_msgs::msg::Image::width() const
+{
+    return m_width;
+}
+
+/*!
+ * @brief This function returns a reference to member width
+ * @return Reference to member width
+ */
+uint32_t& sensor_msgs::msg::Image::width()
+{
+    return m_width;
+}
+
+/*!
+ * @brief This function copies the value in member encoding
+ * @param _encoding New value to be copied in member encoding
+ */
+void sensor_msgs::msg::Image::encoding(
+        const std::string& _encoding)
+{
+    m_encoding = _encoding;
+}
+
+/*!
+ * @brief This function moves the value in member encoding
+ * @param _encoding New value to be moved in member encoding
+ */
+void sensor_msgs::msg::Image::encoding(
+        std::string&& _encoding)
+{
+    m_encoding = std::move(_encoding);
+}
+
+/*!
+ * @brief This function returns a constant reference to member encoding
+ * @return Constant reference to member encoding
+ */
+const std::string& sensor_msgs::msg::Image::encoding() const
+{
+    return m_encoding;
+}
+
+/*!
+ * @brief This function returns a reference to member encoding
+ * @return Reference to member encoding
+ */
+std::string& sensor_msgs::msg::Image::encoding()
+{
+    return m_encoding;
+}
+
+/*!
+ * @brief This function sets a value in member is_bigendian
+ * @param _is_bigendian New value for member is_bigendian
+ */
+void sensor_msgs::msg::Image::is_bigendian(
+        uint8_t _is_bigendian)
+{
+    m_is_bigendian = _is_bigendian;
+}
+
+/*!
+ * @brief This function returns the value of member is_bigendian
+ * @return Value of member is_bigendian
+ */
+uint8_t sensor_msgs::msg::Image::is_bigendian() const
+{
+    return m_is_bigendian;
+}
+
+/*!
+ * @brief This function returns a reference to member is_bigendian
+ * @return Reference to member is_bigendian
+ */
+uint8_t& sensor_msgs::msg::Image::is_bigendian()
+{
+    return m_is_bigendian;
+}
+
+/*!
+ * @brief This function sets a value in member step
+ * @param _step New value for member step
+ */
+void sensor_msgs::msg::Image::step(
+        uint32_t _step)
+{
+    m_step = _step;
+}
+
+/*!
+ * @brief This function returns the value of member step
+ * @return Value of member step
+ */
+uint32_t sensor_msgs::msg::Image::step() const
+{
+    return m_step;
+}
+
+/*!
+ * @brief This function returns a reference to member step
+ * @return Reference to member step
+ */
+uint32_t& sensor_msgs::msg::Image::step()
+{
+    return m_step;
+}
+
+/*!
+ * @brief This function copies the value in member data
+ * @param _data New value to be copied in member data
+ */
+void sensor_msgs::msg::Image::data(
+        const std::vector<uint8_t>& _data)
+{
+    m_data = _data;
+}
+
+/*!
+ * @brief This function moves the value in member data
+ * @param _data New value to be moved in member data
+ */
+void sensor_msgs::msg::Image::data(
+        std::vector<uint8_t>&& _data)
+{
+    m_data = std::move(_data);
+}
+
+/*!
+ * @brief This function returns a constant reference to member data
+ * @return Constant reference to member data
+ */
+const std::vector<uint8_t>& sensor_msgs::msg::Image::data() const
+{
+    return m_data;
+}
+
+/*!
+ * @brief This function returns a reference to member data
+ * @return Reference to member data
+ */
+std::vector<uint8_t>& sensor_msgs::msg::Image::data()
+{
+    return m_data;
+}
+
+size_t sensor_msgs::msg::Image::getKeyMaxCdrSerializedSize(
+        size_t current_alignment)
+{
+    static_cast<void>(current_alignment);
+    return sensor_msgs_msg_Image_max_key_cdr_typesize;
+}
+
+bool sensor_msgs::msg::Image::isKeyDefined()
+{
+    return false;
+}
+
+void sensor_msgs::msg::Image::serializeKey(
+        eprosima::fastcdr::Cdr& scdr) const
+{
+    (void) scdr;
+}

LibCarla/source/carla/ros2/types/Image.h

@@ -0,0 +1,347 @@
+// Copyright 2016 Proyectos y Sistemas de Mantenimiento SL (eProsima).
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+/*!
+ * @file Image.h
+ * This header file contains the declaration of the described types in the IDL file.
+ *
+ * This file was generated by the tool gen.
+ */
+
+#ifndef _FAST_DDS_GENERATED_SENSOR_MSGS_MSG_IMAGE_H_
+#define _FAST_DDS_GENERATED_SENSOR_MSGS_MSG_IMAGE_H_
+
+#include "Header.h"
+
+#include <fastrtps/utils/fixed_size_string.hpp>
+
+#include <stdint.h>
+#include <array>
+#include <string>
+#include <vector>
+#include <map>
+#include <bitset>
+
+#if defined(_WIN32)
+#if defined(EPROSIMA_USER_DLL_EXPORT)
+#define eProsima_user_DllExport __declspec( dllexport )
+#else
+#define eProsima_user_DllExport
+#endif  // EPROSIMA_USER_DLL_EXPORT
+#else
+#define eProsima_user_DllExport
+#endif  // _WIN32
+
+#if defined(_WIN32)
+#if defined(EPROSIMA_USER_DLL_EXPORT)
+#if defined(Image_SOURCE)
+#define Image_DllAPI __declspec( dllexport )
+#else
+#define Image_DllAPI __declspec( dllimport )
+#endif // Image_SOURCE
+#else
+#define Image_DllAPI
+#endif  // EPROSIMA_USER_DLL_EXPORT
+#else
+#define Image_DllAPI
+#endif // _WIN32
+
+namespace eprosima {
+namespace fastcdr {
+class Cdr;
+} // namespace fastcdr
+} // namespace eprosima
+
+namespace sensor_msgs {
+    namespace msg {
+        /*!
+         * @brief This class represents the structure Image defined by the user in the IDL file.
+         * @ingroup IMAGE
+         */
+        class Image
+        {
+        public:
+
+            /*!
+             * @brief Default constructor.
+             */
+            eProsima_user_DllExport Image();
+
+            /*!
+             * @brief Default destructor.
+             */
+            eProsima_user_DllExport ~Image();
+
+            /*!
+             * @brief Copy constructor.
+             * @param x Reference to the object sensor_msgs::msg::Image that will be copied.
+             */
+            eProsima_user_DllExport Image(
+                    const Image& x);
+
+            /*!
+             * @brief Move constructor.
+             * @param x Reference to the object sensor_msgs::msg::Image that will be copied.
+             */
+            eProsima_user_DllExport Image(
+                    Image&& x) noexcept;
+
+            /*!
+             * @brief Copy assignment.
+             * @param x Reference to the object sensor_msgs::msg::Image that will be copied.
+             */
+            eProsima_user_DllExport Image& operator =(
+                    const Image& x);
+
+            /*!
+             * @brief Move assignment.
+             * @param x Reference to the object sensor_msgs::msg::Image that will be copied.
+             */
+            eProsima_user_DllExport Image& operator =(
+                    Image&& x) noexcept;
+
+            /*!
+             * @brief Comparison operator.
+             * @param x sensor_msgs::msg::Image object to compare.
+             */
+            eProsima_user_DllExport bool operator ==(
+                    const Image& x) const;
+
+            /*!
+             * @brief Comparison operator.
+             * @param x sensor_msgs::msg::Image object to compare.
+             */
+            eProsima_user_DllExport bool operator !=(
+                    const Image& x) const;
+
+            /*!
+             * @brief This function copies the value in member header
+             * @param _header New value to be copied in member header
+             */
+            eProsima_user_DllExport void header(
+                    const std_msgs::msg::Header& _header);
+
+            /*!
+             * @brief This function moves the value in member header
+             * @param _header New value to be moved in member header
+             */
+            eProsima_user_DllExport void header(
+                    std_msgs::msg::Header&& _header);
+
+            /*!
+             * @brief This function returns a constant reference to member header
+             * @return Constant reference to member header
+             */
+            eProsima_user_DllExport const std_msgs::msg::Header& header() const;
+
+            /*!
+             * @brief This function returns a reference to member header
+             * @return Reference to member header
+             */
+            eProsima_user_DllExport std_msgs::msg::Header& header();
+            /*!
+             * @brief This function sets a value in member height
+             * @param _height New value for member height
+             */
+            eProsima_user_DllExport void height(
+                    uint32_t _height);
+
+            /*!
+             * @brief This function returns the value of member height
+             * @return Value of member height
+             */
+            eProsima_user_DllExport uint32_t height() const;
+
+            /*!
+             * @brief This function returns a reference to member height
+             * @return Reference to member height
+             */
+            eProsima_user_DllExport uint32_t& height();
+
+            /*!
+             * @brief This function sets a value in member width
+             * @param _width New value for member width
+             */
+            eProsima_user_DllExport void width(
+                    uint32_t _width);
+
+            /*!
+             * @brief This function returns the value of member width
+             * @return Value of member width
+             */
+            eProsima_user_DllExport uint32_t width() const;
+
+            /*!
+             * @brief This function returns a reference to member width
+             * @return Reference to member width
+             */
+            eProsima_user_DllExport uint32_t& width();
+
+            /*!
+             * @brief This function copies the value in member encoding
+             * @param _encoding New value to be copied in member encoding
+             */
+            eProsima_user_DllExport void encoding(
+                    const std::string& _encoding);
+
+            /*!
+             * @brief This function moves the value in member encoding
+             * @param _encoding New value to be moved in member encoding
+             */
+            eProsima_user_DllExport void encoding(
+                    std::string&& _encoding);
+
+            /*!
+             * @brief This function returns a constant reference to member encoding
+             * @return Constant reference to member encoding
+             */
+            eProsima_user_DllExport const std::string& encoding() const;
+
+            /*!
+             * @brief This function returns a reference to member encoding
+             * @return Reference to member encoding
+             */
+            eProsima_user_DllExport std::string& encoding();
+            /*!
+             * @brief This function sets a value in member is_bigendian
+             * @param _is_bigendian New value for member is_bigendian
+             */
+            eProsima_user_DllExport void is_bigendian(
+                    uint8_t _is_bigendian);
+
+            /*!
+             * @brief This function returns the value of member is_bigendian
+             * @return Value of member is_bigendian
+             */
+            eProsima_user_DllExport uint8_t is_bigendian() const;
+
+            /*!
+             * @brief This function returns a reference to member is_bigendian
+             * @return Reference to member is_bigendian
+             */
+            eProsima_user_DllExport uint8_t& is_bigendian();
+
+            /*!
+             * @brief This function sets a value in member step
+             * @param _step New value for member step
+             */
+            eProsima_user_DllExport void step(
+                    uint32_t _step);
+
+            /*!
+             * @brief This function returns the value of member step
+             * @return Value of member step
+             */
+            eProsima_user_DllExport uint32_t step() const;
+
+            /*!
+             * @brief This function returns a reference to member step
+             * @return Reference to member step
+             */
+            eProsima_user_DllExport uint32_t& step();
+
+            /*!
+             * @brief This function copies the value in member data
+             * @param _data New value to be copied in member data
+             */
+            eProsima_user_DllExport void data(
+                    const std::vector<uint8_t>& _data);
+
+            /*!
+             * @brief This function moves the value in member data
+             * @param _data New value to be moved in member data
+             */
+            eProsima_user_DllExport void data(
+                    std::vector<uint8_t>&& _data);
+
+            /*!
+             * @brief This function returns a constant reference to member data
+             * @return Constant reference to member data
+             */
+            eProsima_user_DllExport const std::vector<uint8_t>& data() const;
+
+            /*!
+             * @brief This function returns a reference to member data
+             * @return Reference to member data
+             */
+            eProsima_user_DllExport std::vector<uint8_t>& data();
+
+            /*!
+            * @brief This function returns the maximum serialized size of an object
+            * depending on the buffer alignment.
+            * @param current_alignment Buffer alignment.
+            * @return Maximum serialized size.
+            */
+            eProsima_user_DllExport static size_t getMaxCdrSerializedSize(
+                    size_t current_alignment = 0);
+
+            /*!
+             * @brief This function returns the serialized size of a data depending on the buffer alignment.
+             * @param data Data which is calculated its serialized size.
+             * @param current_alignment Buffer alignment.
+             * @return Serialized size.
+             */
+            eProsima_user_DllExport static size_t getCdrSerializedSize(
+                    const sensor_msgs::msg::Image& data,
+                    size_t current_alignment = 0);
+
+
+            /*!
+             * @brief This function serializes an object using CDR serialization.
+             * @param cdr CDR serialization object.
+             */
+            eProsima_user_DllExport void serialize(
+                    eprosima::fastcdr::Cdr& cdr) const;
+
+            /*!
+             * @brief This function deserializes an object using CDR serialization.
+             * @param cdr CDR serialization object.
+             */
+            eProsima_user_DllExport void deserialize(
+                    eprosima::fastcdr::Cdr& cdr);
+
+            /*!
+             * @brief This function returns the maximum serialized size of the Key of an object
+             * depending on the buffer alignment.
+             * @param current_alignment Buffer alignment.
+             * @return Maximum serialized size.
+             */
+            eProsima_user_DllExport static size_t getKeyMaxCdrSerializedSize(
+                    size_t current_alignment = 0);
+
+            /*!
+             * @brief This function tells you if the Key has been defined for this type
+             */
+            eProsima_user_DllExport static bool isKeyDefined();
+
+            /*!
+             * @brief This function serializes the key members of an object using CDR serialization.
+             * @param cdr CDR serialization object.
+             */
+            eProsima_user_DllExport void serializeKey(
+                    eprosima::fastcdr::Cdr& cdr) const;
+
+        private:
+            std_msgs::msg::Header m_header;
+            uint32_t m_height;
+            uint32_t m_width;
+            std::string m_encoding;
+            uint8_t m_is_bigendian;
+            uint32_t m_step;
+            std::vector<uint8_t> m_data;
+        };
+    } // namespace msg
+} // namespace sensor_msgs
+
+#endif // _FAST_DDS_GENERATED_SENSOR_MSGS_MSG_IMAGE_H_

LibCarla/source/carla/ros2/types/ImagePubSubTypes.cpp

@@ -0,0 +1,154 @@
+// Copyright 2016 Proyectos y Sistemas de Mantenimiento SL (eProsima).
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+/*!
+ * @file ImagePubSubTypes.cpp
+ * This header file contains the implementation of the serialization functions.
+ *
+ * This file was generated by the tool fastcdrgen.
+ */
+
+#include <fastcdr/FastBuffer.h>
+#include <fastcdr/Cdr.h>
+
+#include "ImagePubSubTypes.h"
+
+using SerializedPayload_t = eprosima::fastrtps::rtps::SerializedPayload_t;
+using InstanceHandle_t = eprosima::fastrtps::rtps::InstanceHandle_t;
+
+namespace sensor_msgs {
+    namespace msg {
+        ImagePubSubType::ImagePubSubType()
+        {
+            setName("sensor_msgs::msg::dds_::Image_");
+            auto type_size = Image::getMaxCdrSerializedSize();
+            type_size += eprosima::fastcdr::Cdr::alignment(type_size, 4); /* possible submessage alignment */
+            m_typeSize = static_cast<uint32_t>(type_size) + 4; /*encapsulation*/
+            m_isGetKeyDefined = Image::isKeyDefined();
+            size_t keyLength = Image::getKeyMaxCdrSerializedSize() > 16 ?
+                    Image::getKeyMaxCdrSerializedSize() : 16;
+            m_keyBuffer = reinterpret_cast<unsigned char*>(malloc(keyLength));
+            memset(m_keyBuffer, 0, keyLength);
+        }
+
+        ImagePubSubType::~ImagePubSubType()
+        {
+            if (m_keyBuffer != nullptr)
+            {
+                free(m_keyBuffer);
+            }
+        }
+
+        bool ImagePubSubType::serialize(
+                void* data,
+                SerializedPayload_t* payload)
+        {
+            Image* p_type = static_cast<Image*>(data);
+
+            // Object that manages the raw buffer.
+            eprosima::fastcdr::FastBuffer fastbuffer(reinterpret_cast<char*>(payload->data), payload->max_size);
+            // Object that serializes the data.
+            eprosima::fastcdr::Cdr ser(fastbuffer, eprosima::fastcdr::Cdr::DEFAULT_ENDIAN, eprosima::fastcdr::Cdr::DDS_CDR);
+            payload->encapsulation = ser.endianness() == eprosima::fastcdr::Cdr::BIG_ENDIANNESS ? CDR_BE : CDR_LE;
+            // Serialize encapsulation
+            ser.serialize_encapsulation();
+            p_type->serialize(ser);
+            // Get the serialized length
+            payload->length = static_cast<uint32_t>(ser.getSerializedDataLength());
+            return true;
+        }
+
+        bool ImagePubSubType::deserialize(
+                SerializedPayload_t* payload,
+                void* data)
+        {
+            //Convert DATA to pointer of your type
+            Image* p_type = static_cast<Image*>(data);
+
+            // Object that manages the raw buffer.
+            eprosima::fastcdr::FastBuffer fastbuffer(reinterpret_cast<char*>(payload->data), payload->length);
+
+            // Object that deserializes the data.
+            eprosima::fastcdr::Cdr deser(fastbuffer, eprosima::fastcdr::Cdr::DEFAULT_ENDIAN, eprosima::fastcdr::Cdr::DDS_CDR);
+
+            // Deserialize encapsulation.
+            deser.read_encapsulation();
+            payload->encapsulation = deser.endianness() == eprosima::fastcdr::Cdr::BIG_ENDIANNESS ? CDR_BE : CDR_LE;
+
+            // Deserialize the object.
+            p_type->deserialize(deser);
+            return true;
+        }
+
+        std::function<uint32_t()> ImagePubSubType::getSerializedSizeProvider(
+                void* data)
+        {
+            return [data]() -> uint32_t
+                   {
+                       return static_cast<uint32_t>(type::getCdrSerializedSize(*static_cast<Image*>(data))) +
+                              4u /*encapsulation*/;
+                   };
+        }
+
+        void* ImagePubSubType::createData()
+        {
+            return reinterpret_cast<void*>(new Image());
+        }
+
+        void ImagePubSubType::deleteData(
+                void* data)
+        {
+            delete(reinterpret_cast<Image*>(data));
+        }
+
+        bool ImagePubSubType::getKey(
+                void* data,
+                InstanceHandle_t* handle,
+                bool force_md5)
+        {
+            if (!m_isGetKeyDefined)
+            {
+                return false;
+            }
+
+            Image* p_type = static_cast<Image*>(data);
+
+            // Object that manages the raw buffer.
+            eprosima::fastcdr::FastBuffer fastbuffer(reinterpret_cast<char*>(m_keyBuffer),
+                    Image::getKeyMaxCdrSerializedSize());
+
+            // Object that serializes the data.
+            eprosima::fastcdr::Cdr ser(fastbuffer, eprosima::fastcdr::Cdr::BIG_ENDIANNESS);
+            p_type->serializeKey(ser);
+            if (force_md5 || Image::getKeyMaxCdrSerializedSize() > 16)
+            {
+                m_md5.init();
+                m_md5.update(m_keyBuffer, static_cast<unsigned int>(ser.getSerializedDataLength()));
+                m_md5.finalize();
+                for (uint8_t i = 0; i < 16; ++i)
+                {
+                    handle->value[i] = m_md5.digest[i];
+                }
+            }
+            else
+            {
+                for (uint8_t i = 0; i < 16; ++i)
+                {
+                    handle->value[i] = m_keyBuffer[i];
+                }
+            }
+            return true;
+        }
+    } //End of namespace msg
+} //End of namespace sensor_msgs

LibCarla/source/carla/ros2/types/ImagePubSubTypes.h

@@ -0,0 +1,105 @@
+// Copyright 2016 Proyectos y Sistemas de Mantenimiento SL (eProsima).
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+/*!
+ * @file ImagePubSubTypes.h
+ * This header file contains the declaration of the serialization functions.
+ *
+ * This file was generated by the tool fastcdrgen.
+ */
+
+#ifndef _FAST_DDS_GENERATED_SENSOR_MSGS_MSG_IMAGE_PUBSUBTYPES_H_
+#define _FAST_DDS_GENERATED_SENSOR_MSGS_MSG_IMAGE_PUBSUBTYPES_H_
+
+#include <fastdds/dds/topic/TopicDataType.hpp>
+#include <fastrtps/utils/md5.h>
+
+#include "Image.h"
+
+#if !defined(GEN_API_VER) || (GEN_API_VER != 1)
+#error \
+    Generated Image is not compatible with current installed Fast DDS. Please, regenerate it with fastddsgen.
+#endif  // GEN_API_VER
+
+namespace sensor_msgs
+{
+    namespace msg
+    {
+        /*!
+         * @brief This class represents the TopicDataType of the type Image defined by the user in the IDL file.
+         * @ingroup IMAGE
+         */
+        class ImagePubSubType : public eprosima::fastdds::dds::TopicDataType
+        {
+        public:
+
+            typedef Image type;
+
+            eProsima_user_DllExport ImagePubSubType();
+
+            eProsima_user_DllExport virtual ~ImagePubSubType() override;
+
+            eProsima_user_DllExport virtual bool serialize(
+                    void* data,
+                    eprosima::fastrtps::rtps::SerializedPayload_t* payload) override;
+
+            eProsima_user_DllExport virtual bool deserialize(
+                    eprosima::fastrtps::rtps::SerializedPayload_t* payload,
+                    void* data) override;
+
+            eProsima_user_DllExport virtual std::function<uint32_t()> getSerializedSizeProvider(
+                    void* data) override;
+
+            eProsima_user_DllExport virtual bool getKey(
+                    void* data,
+                    eprosima::fastrtps::rtps::InstanceHandle_t* ihandle,
+                    bool force_md5 = false) override;
+
+            eProsima_user_DllExport virtual void* createData() override;
+
+            eProsima_user_DllExport virtual void deleteData(
+                    void* data) override;
+
+        #ifdef TOPIC_DATA_TYPE_API_HAS_IS_BOUNDED
+            eProsima_user_DllExport inline bool is_bounded() const override
+            {
+                return false;
+            }
+
+        #endif  // TOPIC_DATA_TYPE_API_HAS_IS_BOUNDED
+
+        #ifdef TOPIC_DATA_TYPE_API_HAS_IS_PLAIN
+            eProsima_user_DllExport inline bool is_plain() const override
+            {
+                return false;
+            }
+
+        #endif  // TOPIC_DATA_TYPE_API_HAS_IS_PLAIN
+
+        #ifdef TOPIC_DATA_TYPE_API_HAS_CONSTRUCT_SAMPLE
+            eProsima_user_DllExport inline bool construct_sample(
+                    void* memory) const override
+            {
+                (void)memory;
+                return false;
+            }
+
+        #endif  // TOPIC_DATA_TYPE_API_HAS_CONSTRUCT_SAMPLE
+            MD5 m_md5;
+            unsigned char* m_keyBuffer;
+        };
+    }
+}
+
+#endif // _FAST_DDS_GENERATED_SENSOR_MSGS_MSG_IMAGE_PUBSUBTYPES_H_

LibCarla/source/carla/ros2/types/Imu.cpp

@@ -0,0 +1,465 @@
+// Copyright 2016 Proyectos y Sistemas de Mantenimiento SL (eProsima).
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+/*!
+ * @file Imu.cpp
+ * This source file contains the definition of the described types in the IDL file.
+ *
+ * This file was generated by the tool gen.
+ */
+
+#ifdef _WIN32
+// Remove linker warning LNK4221 on Visual Studio
+namespace {
+char dummy;
+}  // namespace
+#endif  // _WIN32
+
+#include "Imu.h"
+#include <fastcdr/Cdr.h>
+
+#include <fastcdr/exceptions/BadParamException.h>
+using namespace eprosima::fastcdr::exception;
+
+#include <utility>
+
+#define geometry_msgs_msg_Vector3_max_cdr_typesize 24ULL;
+#define std_msgs_msg_Time_max_cdr_typesize 8ULL;
+
+#define geometry_msgs_msg_Quaternion_max_cdr_typesize 32ULL;
+#define std_msgs_msg_Header_max_cdr_typesize 268ULL;
+#define sensor_msgs_msg_Imu_max_cdr_typesize 568ULL;
+#define geometry_msgs_msg_Vector3_max_key_cdr_typesize 0ULL;
+#define std_msgs_msg_Time_max_key_cdr_typesize 0ULL;
+
+#define geometry_msgs_msg_Quaternion_max_key_cdr_typesize 0ULL;
+#define std_msgs_msg_Header_max_key_cdr_typesize 0ULL;
+#define sensor_msgs_msg_Imu_max_key_cdr_typesize 0ULL;
+
+sensor_msgs::msg::Imu::Imu()
+{
+    // std_msgs::msg::Header m_header
+
+    // geometry_msgs::msg::Quaternion m_orientation
+
+    // sensor_msgs::msg::sensor_msgs__Imu__double_array_9 m_orientation_covariance
+    memset(&m_orientation_covariance, 0, (9) * 8);
+    // geometry_msgs::msg::Vector3 m_angular_velocity
+
+    // sensor_msgs::msg::sensor_msgs__Imu__double_array_9 m_angular_velocity_covariance
+    memset(&m_angular_velocity_covariance, 0, (9) * 8);
+    // geometry_msgs::msg::Vector3 m_linear_acceleration
+
+    // sensor_msgs::msg::sensor_msgs__Imu__double_array_9 m_linear_acceleration_covariance
+    memset(&m_linear_acceleration_covariance, 0, (9) * 8);
+}
+
+sensor_msgs::msg::Imu::~Imu()
+{
+}
+
+sensor_msgs::msg::Imu::Imu(
+        const Imu& x)
+{
+    m_header = x.m_header;
+    m_orientation = x.m_orientation;
+    m_orientation_covariance = x.m_orientation_covariance;
+    m_angular_velocity = x.m_angular_velocity;
+    m_angular_velocity_covariance = x.m_angular_velocity_covariance;
+    m_linear_acceleration = x.m_linear_acceleration;
+    m_linear_acceleration_covariance = x.m_linear_acceleration_covariance;
+}
+
+sensor_msgs::msg::Imu::Imu(
+        Imu&& x) noexcept
+{
+    m_header = std::move(x.m_header);
+    m_orientation = std::move(x.m_orientation);
+    m_orientation_covariance = std::move(x.m_orientation_covariance);
+    m_angular_velocity = std::move(x.m_angular_velocity);
+    m_angular_velocity_covariance = std::move(x.m_angular_velocity_covariance);
+    m_linear_acceleration = std::move(x.m_linear_acceleration);
+    m_linear_acceleration_covariance = std::move(x.m_linear_acceleration_covariance);
+}
+
+sensor_msgs::msg::Imu& sensor_msgs::msg::Imu::operator =(
+        const Imu& x)
+{
+    m_header = x.m_header;
+    m_orientation = x.m_orientation;
+    m_orientation_covariance = x.m_orientation_covariance;
+    m_angular_velocity = x.m_angular_velocity;
+    m_angular_velocity_covariance = x.m_angular_velocity_covariance;
+    m_linear_acceleration = x.m_linear_acceleration;
+    m_linear_acceleration_covariance = x.m_linear_acceleration_covariance;
+
+    return *this;
+}
+
+sensor_msgs::msg::Imu& sensor_msgs::msg::Imu::operator =(
+        Imu&& x) noexcept
+{
+    m_header = std::move(x.m_header);
+    m_orientation = std::move(x.m_orientation);
+    m_orientation_covariance = std::move(x.m_orientation_covariance);
+    m_angular_velocity = std::move(x.m_angular_velocity);
+    m_angular_velocity_covariance = std::move(x.m_angular_velocity_covariance);
+    m_linear_acceleration = std::move(x.m_linear_acceleration);
+    m_linear_acceleration_covariance = std::move(x.m_linear_acceleration_covariance);
+
+    return *this;
+}
+
+bool sensor_msgs::msg::Imu::operator ==(
+        const Imu& x) const
+{
+    return (m_header == x.m_header && m_orientation == x.m_orientation && m_orientation_covariance == x.m_orientation_covariance && m_angular_velocity == x.m_angular_velocity && m_angular_velocity_covariance == x.m_angular_velocity_covariance && m_linear_acceleration == x.m_linear_acceleration && m_linear_acceleration_covariance == x.m_linear_acceleration_covariance);
+}
+
+bool sensor_msgs::msg::Imu::operator !=(
+        const Imu& x) const
+{
+    return !(*this == x);
+}
+
+size_t sensor_msgs::msg::Imu::getMaxCdrSerializedSize(
+        size_t current_alignment)
+{
+    static_cast<void>(current_alignment);
+    return sensor_msgs_msg_Imu_max_cdr_typesize;
+}
+
+size_t sensor_msgs::msg::Imu::getCdrSerializedSize(
+        const sensor_msgs::msg::Imu& data,
+        size_t current_alignment)
+{
+    size_t initial_alignment = current_alignment;
+    current_alignment += std_msgs::msg::Header::getCdrSerializedSize(data.header(), current_alignment);
+    current_alignment += geometry_msgs::msg::Quaternion::getCdrSerializedSize(data.orientation(), current_alignment);
+    current_alignment += ((9) * 8) + eprosima::fastcdr::Cdr::alignment(current_alignment, 8);
+    current_alignment += geometry_msgs::msg::Vector3::getCdrSerializedSize(data.angular_velocity(), current_alignment);
+    current_alignment += ((9) * 8) + eprosima::fastcdr::Cdr::alignment(current_alignment, 8);
+    current_alignment += geometry_msgs::msg::Vector3::getCdrSerializedSize(data.linear_acceleration(), current_alignment);
+    current_alignment += ((9) * 8) + eprosima::fastcdr::Cdr::alignment(current_alignment, 8);
+
+    return current_alignment - initial_alignment;
+}
+
+void sensor_msgs::msg::Imu::serialize(
+        eprosima::fastcdr::Cdr& scdr) const
+{
+    scdr << m_header;
+    scdr << m_orientation;
+    scdr << m_orientation_covariance;
+    scdr << m_angular_velocity;
+    scdr << m_angular_velocity_covariance;
+    scdr << m_linear_acceleration;
+    scdr << m_linear_acceleration_covariance;
+}
+
+void sensor_msgs::msg::Imu::deserialize(
+        eprosima::fastcdr::Cdr& dcdr)
+{
+    dcdr >> m_header;
+    dcdr >> m_orientation;
+    dcdr >> m_orientation_covariance;
+    dcdr >> m_angular_velocity;
+    dcdr >> m_angular_velocity_covariance;
+    dcdr >> m_linear_acceleration;
+    dcdr >> m_linear_acceleration_covariance;
+}
+
+/*!
+ * @brief This function copies the value in member header
+ * @param _header New value to be copied in member header
+ */
+void sensor_msgs::msg::Imu::header(
+        const std_msgs::msg::Header& _header)
+{
+    m_header = _header;
+}
+
+/*!
+ * @brief This function moves the value in member header
+ * @param _header New value to be moved in member header
+ */
+void sensor_msgs::msg::Imu::header(
+        std_msgs::msg::Header&& _header)
+{
+    m_header = std::move(_header);
+}
+
+/*!
+ * @brief This function returns a constant reference to member header
+ * @return Constant reference to member header
+ */
+const std_msgs::msg::Header& sensor_msgs::msg::Imu::header() const
+{
+    return m_header;
+}
+
+/*!
+ * @brief This function returns a reference to member header
+ * @return Reference to member header
+ */
+std_msgs::msg::Header& sensor_msgs::msg::Imu::header()
+{
+    return m_header;
+}
+
+/*!
+ * @brief This function copies the value in member orientation
+ * @param _orientation New value to be copied in member orientation
+ */
+void sensor_msgs::msg::Imu::orientation(
+        const geometry_msgs::msg::Quaternion& _orientation)
+{
+    m_orientation = _orientation;
+}
+
+/*!
+ * @brief This function moves the value in member orientation
+ * @param _orientation New value to be moved in member orientation
+ */
+void sensor_msgs::msg::Imu::orientation(
+        geometry_msgs::msg::Quaternion&& _orientation)
+{
+    m_orientation = std::move(_orientation);
+}
+
+/*!
+ * @brief This function returns a constant reference to member orientation
+ * @return Constant reference to member orientation
+ */
+const geometry_msgs::msg::Quaternion& sensor_msgs::msg::Imu::orientation() const
+{
+    return m_orientation;
+}
+
+/*!
+ * @brief This function returns a reference to member orientation
+ * @return Reference to member orientation
+ */
+geometry_msgs::msg::Quaternion& sensor_msgs::msg::Imu::orientation()
+{
+    return m_orientation;
+}
+
+/*!
+ * @brief This function copies the value in member orientation_covariance
+ * @param _orientation_covariance New value to be copied in member orientation_covariance
+ */
+void sensor_msgs::msg::Imu::orientation_covariance(
+        const sensor_msgs::msg::sensor_msgs__Imu__double_array_9& _orientation_covariance)
+{
+    m_orientation_covariance = _orientation_covariance;
+}
+
+/*!
+ * @brief This function moves the value in member orientation_covariance
+ * @param _orientation_covariance New value to be moved in member orientation_covariance
+ */
+void sensor_msgs::msg::Imu::orientation_covariance(
+        sensor_msgs::msg::sensor_msgs__Imu__double_array_9&& _orientation_covariance)
+{
+    m_orientation_covariance = std::move(_orientation_covariance);
+}
+
+/*!
+ * @brief This function returns a constant reference to member orientation_covariance
+ * @return Constant reference to member orientation_covariance
+ */
+const sensor_msgs::msg::sensor_msgs__Imu__double_array_9& sensor_msgs::msg::Imu::orientation_covariance() const
+{
+    return m_orientation_covariance;
+}
+
+/*!
+ * @brief This function returns a reference to member orientation_covariance
+ * @return Reference to member orientation_covariance
+ */
+sensor_msgs::msg::sensor_msgs__Imu__double_array_9& sensor_msgs::msg::Imu::orientation_covariance()
+{
+    return m_orientation_covariance;
+}
+
+/*!
+ * @brief This function copies the value in member angular_velocity
+ * @param _angular_velocity New value to be copied in member angular_velocity
+ */
+void sensor_msgs::msg::Imu::angular_velocity(
+        const geometry_msgs::msg::Vector3& _angular_velocity)
+{
+    m_angular_velocity = _angular_velocity;
+}
+
+/*!
+ * @brief This function moves the value in member angular_velocity
+ * @param _angular_velocity New value to be moved in member angular_velocity
+ */
+void sensor_msgs::msg::Imu::angular_velocity(
+        geometry_msgs::msg::Vector3&& _angular_velocity)
+{
+    m_angular_velocity = std::move(_angular_velocity);
+}
+
+/*!
+ * @brief This function returns a constant reference to member angular_velocity
+ * @return Constant reference to member angular_velocity
+ */
+const geometry_msgs::msg::Vector3& sensor_msgs::msg::Imu::angular_velocity() const
+{
+    return m_angular_velocity;
+}
+
+/*!
+ * @brief This function returns a reference to member angular_velocity
+ * @return Reference to member angular_velocity
+ */
+geometry_msgs::msg::Vector3& sensor_msgs::msg::Imu::angular_velocity()
+{
+    return m_angular_velocity;
+}
+
+/*!
+ * @brief This function copies the value in member angular_velocity_covariance
+ * @param _angular_velocity_covariance New value to be copied in member angular_velocity_covariance
+ */
+void sensor_msgs::msg::Imu::angular_velocity_covariance(
+        const sensor_msgs::msg::sensor_msgs__Imu__double_array_9& _angular_velocity_covariance)
+{
+    m_angular_velocity_covariance = _angular_velocity_covariance;
+}
+
+/*!
+ * @brief This function moves the value in member angular_velocity_covariance
+ * @param _angular_velocity_covariance New value to be moved in member angular_velocity_covariance
+ */
+void sensor_msgs::msg::Imu::angular_velocity_covariance(
+        sensor_msgs::msg::sensor_msgs__Imu__double_array_9&& _angular_velocity_covariance)
+{
+    m_angular_velocity_covariance = std::move(_angular_velocity_covariance);
+}
+
+/*!
+ * @brief This function returns a constant reference to member angular_velocity_covariance
+ * @return Constant reference to member angular_velocity_covariance
+ */
+const sensor_msgs::msg::sensor_msgs__Imu__double_array_9& sensor_msgs::msg::Imu::angular_velocity_covariance() const
+{
+    return m_angular_velocity_covariance;
+}
+
+/*!
+ * @brief This function returns a reference to member angular_velocity_covariance
+ * @return Reference to member angular_velocity_covariance
+ */
+sensor_msgs::msg::sensor_msgs__Imu__double_array_9& sensor_msgs::msg::Imu::angular_velocity_covariance()
+{
+    return m_angular_velocity_covariance;
+}
+
+/*!
+ * @brief This function copies the value in member linear_acceleration
+ * @param _linear_acceleration New value to be copied in member linear_acceleration
+ */
+void sensor_msgs::msg::Imu::linear_acceleration(
+        const geometry_msgs::msg::Vector3& _linear_acceleration)
+{
+    m_linear_acceleration = _linear_acceleration;
+}
+
+/*!
+ * @brief This function moves the value in member linear_acceleration
+ * @param _linear_acceleration New value to be moved in member linear_acceleration
+ */
+void sensor_msgs::msg::Imu::linear_acceleration(
+        geometry_msgs::msg::Vector3&& _linear_acceleration)
+{
+    m_linear_acceleration = std::move(_linear_acceleration);
+}
+
+/*!
+ * @brief This function returns a constant reference to member linear_acceleration
+ * @return Constant reference to member linear_acceleration
+ */
+const geometry_msgs::msg::Vector3& sensor_msgs::msg::Imu::linear_acceleration() const
+{
+    return m_linear_acceleration;
+}
+
+/*!
+ * @brief This function returns a reference to member linear_acceleration
+ * @return Reference to member linear_acceleration
+ */
+geometry_msgs::msg::Vector3& sensor_msgs::msg::Imu::linear_acceleration()
+{
+    return m_linear_acceleration;
+}
+
+/*!
+ * @brief This function copies the value in member linear_acceleration_covariance
+ * @param _linear_acceleration_covariance New value to be copied in member linear_acceleration_covariance
+ */
+void sensor_msgs::msg::Imu::linear_acceleration_covariance(
+        const sensor_msgs::msg::sensor_msgs__Imu__double_array_9& _linear_acceleration_covariance)
+{
+    m_linear_acceleration_covariance = _linear_acceleration_covariance;
+}
+
+/*!
+ * @brief This function moves the value in member linear_acceleration_covariance
+ * @param _linear_acceleration_covariance New value to be moved in member linear_acceleration_covariance
+ */
+void sensor_msgs::msg::Imu::linear_acceleration_covariance(
+        sensor_msgs::msg::sensor_msgs__Imu__double_array_9&& _linear_acceleration_covariance)
+{
+    m_linear_acceleration_covariance = std::move(_linear_acceleration_covariance);
+}
+
+/*!
+ * @brief This function returns a constant reference to member linear_acceleration_covariance
+ * @return Constant reference to member linear_acceleration_covariance
+ */
+const sensor_msgs::msg::sensor_msgs__Imu__double_array_9& sensor_msgs::msg::Imu::linear_acceleration_covariance() const
+{
+    return m_linear_acceleration_covariance;
+}
+
+/*!
+ * @brief This function returns a reference to member linear_acceleration_covariance
+ * @return Reference to member linear_acceleration_covariance
+ */
+sensor_msgs::msg::sensor_msgs__Imu__double_array_9& sensor_msgs::msg::Imu::linear_acceleration_covariance()
+{
+    return m_linear_acceleration_covariance;
+}
+
+size_t sensor_msgs::msg::Imu::getKeyMaxCdrSerializedSize(
+        size_t current_alignment)
+{
+    static_cast<void>(current_alignment);
+    return sensor_msgs_msg_Imu_max_key_cdr_typesize;
+}
+
+bool sensor_msgs::msg::Imu::isKeyDefined()
+{
+    return false;
+}
+
+void sensor_msgs::msg::Imu::serializeKey(
+        eprosima::fastcdr::Cdr& scdr) const
+{
+    (void) scdr;
+}

LibCarla/source/carla/ros2/types/Imu.h

@@ -0,0 +1,373 @@
+// Copyright 2016 Proyectos y Sistemas de Mantenimiento SL (eProsima).
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+/*!
+ * @file Imu.h
+ * This header file contains the declaration of the described types in the IDL file.
+ *
+ * This file was generated by the tool gen.
+ */
+
+#ifndef _FAST_DDS_GENERATED_SENSOR_MSGS_MSG_IMU_H_
+#define _FAST_DDS_GENERATED_SENSOR_MSGS_MSG_IMU_H_
+
+#include "Vector3.h"
+#include "Quaternion.h"
+#include "Header.h"
+
+#include <fastrtps/utils/fixed_size_string.hpp>
+
+#include <stdint.h>
+#include <array>
+#include <string>
+#include <vector>
+#include <map>
+#include <bitset>
+
+#if defined(_WIN32)
+#if defined(EPROSIMA_USER_DLL_EXPORT)
+#define eProsima_user_DllExport __declspec( dllexport )
+#else
+#define eProsima_user_DllExport
+#endif  // EPROSIMA_USER_DLL_EXPORT
+#else
+#define eProsima_user_DllExport
+#endif  // _WIN32
+
+#if defined(_WIN32)
+#if defined(EPROSIMA_USER_DLL_EXPORT)
+#if defined(Imu_SOURCE)
+#define Imu_DllAPI __declspec( dllexport )
+#else
+#define Imu_DllAPI __declspec( dllimport )
+#endif // Imu_SOURCE
+#else
+#define Imu_DllAPI
+#endif  // EPROSIMA_USER_DLL_EXPORT
+#else
+#define Imu_DllAPI
+#endif // _WIN32
+
+namespace eprosima {
+namespace fastcdr {
+class Cdr;
+} // namespace fastcdr
+} // namespace eprosima
+
+namespace sensor_msgs {
+    namespace msg {
+        typedef std::array<double, 9> sensor_msgs__Imu__double_array_9;
+        /*!
+         * @brief This class represents the structure Imu defined by the user in the IDL file.
+         * @ingroup IMU
+         */
+        class Imu
+        {
+        public:
+
+            /*!
+             * @brief Default constructor.
+             */
+            eProsima_user_DllExport Imu();
+
+            /*!
+             * @brief Default destructor.
+             */
+            eProsima_user_DllExport ~Imu();
+
+            /*!
+             * @brief Copy constructor.
+             * @param x Reference to the object sensor_msgs::msg::Imu that will be copied.
+             */
+            eProsima_user_DllExport Imu(
+                    const Imu& x);
+
+            /*!
+             * @brief Move constructor.
+             * @param x Reference to the object sensor_msgs::msg::Imu that will be copied.
+             */
+            eProsima_user_DllExport Imu(
+                    Imu&& x) noexcept;
+
+            /*!
+             * @brief Copy assignment.
+             * @param x Reference to the object sensor_msgs::msg::Imu that will be copied.
+             */
+            eProsima_user_DllExport Imu& operator =(
+                    const Imu& x);
+
+            /*!
+             * @brief Move assignment.
+             * @param x Reference to the object sensor_msgs::msg::Imu that will be copied.
+             */
+            eProsima_user_DllExport Imu& operator =(
+                    Imu&& x) noexcept;
+
+            /*!
+             * @brief Comparison operator.
+             * @param x sensor_msgs::msg::Imu object to compare.
+             */
+            eProsima_user_DllExport bool operator ==(
+                    const Imu& x) const;
+
+            /*!
+             * @brief Comparison operator.
+             * @param x sensor_msgs::msg::Imu object to compare.
+             */
+            eProsima_user_DllExport bool operator !=(
+                    const Imu& x) const;
+
+            /*!
+             * @brief This function copies the value in member header
+             * @param _header New value to be copied in member header
+             */
+            eProsima_user_DllExport void header(
+                    const std_msgs::msg::Header& _header);
+
+            /*!
+             * @brief This function moves the value in member header
+             * @param _header New value to be moved in member header
+             */
+            eProsima_user_DllExport void header(
+                    std_msgs::msg::Header&& _header);
+
+            /*!
+             * @brief This function returns a constant reference to member header
+             * @return Constant reference to member header
+             */
+            eProsima_user_DllExport const std_msgs::msg::Header& header() const;
+
+            /*!
+             * @brief This function returns a reference to member header
+             * @return Reference to member header
+             */
+            eProsima_user_DllExport std_msgs::msg::Header& header();
+            /*!
+             * @brief This function copies the value in member orientation
+             * @param _orientation New value to be copied in member orientation
+             */
+            eProsima_user_DllExport void orientation(
+                    const geometry_msgs::msg::Quaternion& _orientation);
+
+            /*!
+             * @brief This function moves the value in member orientation
+             * @param _orientation New value to be moved in member orientation
+             */
+            eProsima_user_DllExport void orientation(
+                    geometry_msgs::msg::Quaternion&& _orientation);
+
+            /*!
+             * @brief This function returns a constant reference to member orientation
+             * @return Constant reference to member orientation
+             */
+            eProsima_user_DllExport const geometry_msgs::msg::Quaternion& orientation() const;
+
+            /*!
+             * @brief This function returns a reference to member orientation
+             * @return Reference to member orientation
+             */
+            eProsima_user_DllExport geometry_msgs::msg::Quaternion& orientation();
+            /*!
+             * @brief This function copies the value in member orientation_covariance
+             * @param _orientation_covariance New value to be copied in member orientation_covariance
+             */
+            eProsima_user_DllExport void orientation_covariance(
+                    const sensor_msgs::msg::sensor_msgs__Imu__double_array_9& _orientation_covariance);
+
+            /*!
+             * @brief This function moves the value in member orientation_covariance
+             * @param _orientation_covariance New value to be moved in member orientation_covariance
+             */
+            eProsima_user_DllExport void orientation_covariance(
+                    sensor_msgs::msg::sensor_msgs__Imu__double_array_9&& _orientation_covariance);
+
+            /*!
+             * @brief This function returns a constant reference to member orientation_covariance
+             * @return Constant reference to member orientation_covariance
+             */
+            eProsima_user_DllExport const sensor_msgs::msg::sensor_msgs__Imu__double_array_9& orientation_covariance() const;
+
+            /*!
+             * @brief This function returns a reference to member orientation_covariance
+             * @return Reference to member orientation_covariance
+             */
+            eProsima_user_DllExport sensor_msgs::msg::sensor_msgs__Imu__double_array_9& orientation_covariance();
+            /*!
+             * @brief This function copies the value in member angular_velocity
+             * @param _angular_velocity New value to be copied in member angular_velocity
+             */
+            eProsima_user_DllExport void angular_velocity(
+                    const geometry_msgs::msg::Vector3& _angular_velocity);
+
+            /*!
+             * @brief This function moves the value in member angular_velocity
+             * @param _angular_velocity New value to be moved in member angular_velocity
+             */
+            eProsima_user_DllExport void angular_velocity(
+                    geometry_msgs::msg::Vector3&& _angular_velocity);
+
+            /*!
+             * @brief This function returns a constant reference to member angular_velocity
+             * @return Constant reference to member angular_velocity
+             */
+            eProsima_user_DllExport const geometry_msgs::msg::Vector3& angular_velocity() const;
+
+            /*!
+             * @brief This function returns a reference to member angular_velocity
+             * @return Reference to member angular_velocity
+             */
+            eProsima_user_DllExport geometry_msgs::msg::Vector3& angular_velocity();
+            /*!
+             * @brief This function copies the value in member angular_velocity_covariance
+             * @param _angular_velocity_covariance New value to be copied in member angular_velocity_covariance
+             */
+            eProsima_user_DllExport void angular_velocity_covariance(
+                    const sensor_msgs::msg::sensor_msgs__Imu__double_array_9& _angular_velocity_covariance);
+
+            /*!
+             * @brief This function moves the value in member angular_velocity_covariance
+             * @param _angular_velocity_covariance New value to be moved in member angular_velocity_covariance
+             */
+            eProsima_user_DllExport void angular_velocity_covariance(
+                    sensor_msgs::msg::sensor_msgs__Imu__double_array_9&& _angular_velocity_covariance);
+
+            /*!
+             * @brief This function returns a constant reference to member angular_velocity_covariance
+             * @return Constant reference to member angular_velocity_covariance
+             */
+            eProsima_user_DllExport const sensor_msgs::msg::sensor_msgs__Imu__double_array_9& angular_velocity_covariance() const;
+
+            /*!
+             * @brief This function returns a reference to member angular_velocity_covariance
+             * @return Reference to member angular_velocity_covariance
+             */
+            eProsima_user_DllExport sensor_msgs::msg::sensor_msgs__Imu__double_array_9& angular_velocity_covariance();
+            /*!
+             * @brief This function copies the value in member linear_acceleration
+             * @param _linear_acceleration New value to be copied in member linear_acceleration
+             */
+            eProsima_user_DllExport void linear_acceleration(
+                    const geometry_msgs::msg::Vector3& _linear_acceleration);
+
+            /*!
+             * @brief This function moves the value in member linear_acceleration
+             * @param _linear_acceleration New value to be moved in member linear_acceleration
+             */
+            eProsima_user_DllExport void linear_acceleration(
+                    geometry_msgs::msg::Vector3&& _linear_acceleration);
+
+            /*!
+             * @brief This function returns a constant reference to member linear_acceleration
+             * @return Constant reference to member linear_acceleration
+             */
+            eProsima_user_DllExport const geometry_msgs::msg::Vector3& linear_acceleration() const;
+
+            /*!
+             * @brief This function returns a reference to member linear_acceleration
+             * @return Reference to member linear_acceleration
+             */
+            eProsima_user_DllExport geometry_msgs::msg::Vector3& linear_acceleration();
+            /*!
+             * @brief This function copies the value in member linear_acceleration_covariance
+             * @param _linear_acceleration_covariance New value to be copied in member linear_acceleration_covariance
+             */
+            eProsima_user_DllExport void linear_acceleration_covariance(
+                    const sensor_msgs::msg::sensor_msgs__Imu__double_array_9& _linear_acceleration_covariance);
+
+            /*!
+             * @brief This function moves the value in member linear_acceleration_covariance
+             * @param _linear_acceleration_covariance New value to be moved in member linear_acceleration_covariance
+             */
+            eProsima_user_DllExport void linear_acceleration_covariance(
+                    sensor_msgs::msg::sensor_msgs__Imu__double_array_9&& _linear_acceleration_covariance);
+
+            /*!
+             * @brief This function returns a constant reference to member linear_acceleration_covariance
+             * @return Constant reference to member linear_acceleration_covariance
+             */
+            eProsima_user_DllExport const sensor_msgs::msg::sensor_msgs__Imu__double_array_9& linear_acceleration_covariance() const;
+
+            /*!
+             * @brief This function returns a reference to member linear_acceleration_covariance
+             * @return Reference to member linear_acceleration_covariance
+             */
+            eProsima_user_DllExport sensor_msgs::msg::sensor_msgs__Imu__double_array_9& linear_acceleration_covariance();
+
+            /*!
+            * @brief This function returns the maximum serialized size of an object
+            * depending on the buffer alignment.
+            * @param current_alignment Buffer alignment.
+            * @return Maximum serialized size.
+            */
+            eProsima_user_DllExport static size_t getMaxCdrSerializedSize(
+                    size_t current_alignment = 0);
+
+            /*!
+             * @brief This function returns the serialized size of a data depending on the buffer alignment.
+             * @param data Data which is calculated its serialized size.
+             * @param current_alignment Buffer alignment.
+             * @return Serialized size.
+             */
+            eProsima_user_DllExport static size_t getCdrSerializedSize(
+                    const sensor_msgs::msg::Imu& data,
+                    size_t current_alignment = 0);
+
+            /*!
+             * @brief This function serializes an object using CDR serialization.
+             * @param cdr CDR serialization object.
+             */
+            eProsima_user_DllExport void serialize(
+                    eprosima::fastcdr::Cdr& cdr) const;
+
+            /*!
+             * @brief This function deserializes an object using CDR serialization.
+             * @param cdr CDR serialization object.
+             */
+            eProsima_user_DllExport void deserialize(
+                    eprosima::fastcdr::Cdr& cdr);
+
+            /*!
+             * @brief This function returns the maximum serialized size of the Key of an object
+             * depending on the buffer alignment.
+             * @param current_alignment Buffer alignment.
+             * @return Maximum serialized size.
+             */
+            eProsima_user_DllExport static size_t getKeyMaxCdrSerializedSize(
+                    size_t current_alignment = 0);
+
+            /*!
+             * @brief This function tells you if the Key has been defined for this type
+             */
+            eProsima_user_DllExport static bool isKeyDefined();
+
+            /*!
+             * @brief This function serializes the key members of an object using CDR serialization.
+             * @param cdr CDR serialization object.
+             */
+            eProsima_user_DllExport void serializeKey(
+                    eprosima::fastcdr::Cdr& cdr) const;
+
+        private:
+            std_msgs::msg::Header m_header;
+            geometry_msgs::msg::Quaternion m_orientation;
+            sensor_msgs::msg::sensor_msgs__Imu__double_array_9 m_orientation_covariance;
+            geometry_msgs::msg::Vector3 m_angular_velocity;
+            sensor_msgs::msg::sensor_msgs__Imu__double_array_9 m_angular_velocity_covariance;
+            geometry_msgs::msg::Vector3 m_linear_acceleration;
+            sensor_msgs::msg::sensor_msgs__Imu__double_array_9 m_linear_acceleration_covariance;
+        };
+    } // namespace msg
+} // namespace sensor_msgs
+
+#endif // _FAST_DDS_GENERATED_SENSOR_MSGS_MSG_IMU_H_

LibCarla/source/carla/ros2/types/ImuPubSubTypes.cpp

@@ -0,0 +1,173 @@
+// Copyright 2016 Proyectos y Sistemas de Mantenimiento SL (eProsima).
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+/*!
+ * @file ImuPubSubTypes.cpp
+ * This header file contains the implementation of the serialization functions.
+ *
+ * This file was generated by the tool fastcdrgen.
+ */
+
+#include <fastcdr/FastBuffer.h>
+#include <fastcdr/Cdr.h>
+
+#include "ImuPubSubTypes.h"
+
+using SerializedPayload_t = eprosima::fastrtps::rtps::SerializedPayload_t;
+using InstanceHandle_t = eprosima::fastrtps::rtps::InstanceHandle_t;
+
+namespace sensor_msgs {
+    namespace msg {
+
+        ImuPubSubType::ImuPubSubType()
+        {
+            setName("sensor_msgs::msg::dds_::Imu_");
+            auto type_size = Imu::getMaxCdrSerializedSize();
+            type_size += eprosima::fastcdr::Cdr::alignment(type_size, 4); /* possible submessage alignment */
+            m_typeSize = static_cast<uint32_t>(type_size) + 4; /*encapsulation*/
+            m_isGetKeyDefined = Imu::isKeyDefined();
+            size_t keyLength = Imu::getKeyMaxCdrSerializedSize() > 16 ?
+                    Imu::getKeyMaxCdrSerializedSize() : 16;
+            m_keyBuffer = reinterpret_cast<unsigned char*>(malloc(keyLength));
+            memset(m_keyBuffer, 0, keyLength);
+        }
+
+        ImuPubSubType::~ImuPubSubType()
+        {
+            if (m_keyBuffer != nullptr)
+            {
+                free(m_keyBuffer);
+            }
+        }
+
+        bool ImuPubSubType::serialize(
+                void* data,
+                SerializedPayload_t* payload)
+        {
+            Imu* p_type = static_cast<Imu*>(data);
+
+            // Object that manages the raw buffer.
+            eprosima::fastcdr::FastBuffer fastbuffer(reinterpret_cast<char*>(payload->data), payload->max_size);
+            // Object that serializes the data.
+            eprosima::fastcdr::Cdr ser(fastbuffer, eprosima::fastcdr::Cdr::DEFAULT_ENDIAN, eprosima::fastcdr::Cdr::DDS_CDR);
+            payload->encapsulation = ser.endianness() == eprosima::fastcdr::Cdr::BIG_ENDIANNESS ? CDR_BE : CDR_LE;
+            // Serialize encapsulation
+            ser.serialize_encapsulation();
+
+            try
+            {
+                // Serialize the object.
+                p_type->serialize(ser);
+            }
+            catch (eprosima::fastcdr::exception::NotEnoughMemoryException& /*exception*/)
+            {
+                return false;
+            }
+
+            // Get the serialized length
+            payload->length = static_cast<uint32_t>(ser.getSerializedDataLength());
+            return true;
+        }
+
+        bool ImuPubSubType::deserialize(
+                SerializedPayload_t* payload,
+                void* data)
+        {
+            try
+            {
+                //Convert DATA to pointer of your type
+                Imu* p_type = static_cast<Imu*>(data);
+
+                // Object that manages the raw buffer.
+                eprosima::fastcdr::FastBuffer fastbuffer(reinterpret_cast<char*>(payload->data), payload->length);
+
+                // Object that deserializes the data.
+                eprosima::fastcdr::Cdr deser(fastbuffer, eprosima::fastcdr::Cdr::DEFAULT_ENDIAN, eprosima::fastcdr::Cdr::DDS_CDR);
+
+                // Deserialize encapsulation.
+                deser.read_encapsulation();
+                payload->encapsulation = deser.endianness() == eprosima::fastcdr::Cdr::BIG_ENDIANNESS ? CDR_BE : CDR_LE;
+
+                // Deserialize the object.
+                p_type->deserialize(deser);
+            }
+            catch (eprosima::fastcdr::exception::NotEnoughMemoryException& /*exception*/)
+            {
+                return false;
+            }
+
+            return true;
+        }
+
+        std::function<uint32_t()> ImuPubSubType::getSerializedSizeProvider(
+                void* data)
+        {
+            return [data]() -> uint32_t
+                   {
+                       return static_cast<uint32_t>(type::getCdrSerializedSize(*static_cast<Imu*>(data))) +
+                              4u /*encapsulation*/;
+                   };
+        }
+
+        void* ImuPubSubType::createData()
+        {
+            return reinterpret_cast<void*>(new Imu());
+        }
+
+        void ImuPubSubType::deleteData(
+                void* data)
+        {
+            delete(reinterpret_cast<Imu*>(data));
+        }
+
+        bool ImuPubSubType::getKey(
+                void* data,
+                InstanceHandle_t* handle,
+                bool force_md5)
+        {
+            if (!m_isGetKeyDefined)
+            {
+                return false;
+            }
+
+            Imu* p_type = static_cast<Imu*>(data);
+
+            // Object that manages the raw buffer.
+            eprosima::fastcdr::FastBuffer fastbuffer(reinterpret_cast<char*>(m_keyBuffer),
+                    Imu::getKeyMaxCdrSerializedSize());
+
+            // Object that serializes the data.
+            eprosima::fastcdr::Cdr ser(fastbuffer, eprosima::fastcdr::Cdr::BIG_ENDIANNESS);
+            p_type->serializeKey(ser);
+            if (force_md5 || Imu::getKeyMaxCdrSerializedSize() > 16)
+            {
+                m_md5.init();
+                m_md5.update(m_keyBuffer, static_cast<unsigned int>(ser.getSerializedDataLength()));
+                m_md5.finalize();
+                for (uint8_t i = 0; i < 16; ++i)
+                {
+                    handle->value[i] = m_md5.digest[i];
+                }
+            }
+            else
+            {
+                for (uint8_t i = 0; i < 16; ++i)
+                {
+                    handle->value[i] = m_keyBuffer[i];
+                }
+            }
+            return true;
+        }
+    } //End of namespace msg
+} //End of namespace sensor_msgs