diff --git a/CHANGELOG.md b/CHANGELOG.md
index d743c9c7e..7340e096a 100644
--- a/CHANGELOG.md
+++ b/CHANGELOG.md
@@ -12,13 +12,11 @@
  * Fixed bug in python agents when vehicle list was empty causing a check on all vehicles (BasicAgent.py) and detected pedestrians as vehicles if no pedestrains are present (BehaviourAgent.py) 
  * Extended debug drawing functions to allow drawing primitives on HUD layer
  * Added possibility to change gravity variable in imui sensor for the accelerometer
- * Fixed ROS2 native extension build error when ROS2 is installed in the system.
- * ROS2Native: Force fast-dds dependencies download to avoid build crash when boost_asio and tinyxml2 are not installed in Linux.
  * Added API function `get_telemetry_data` to the vehicle actor.
  * PythonAPI `Sensor.is_listening` was defined twice (property and method), cleaned and clarified it as a method.
  * Added V2X sensors for cooperative awareness message and custom user-defined messages to support vehicle-to-vehicle communication
  * Added named tuples for BasicAgent.py's detection result to allow for type-hints and better semantics.
-
+ * Cleaned up the client/server build and respective naming
 
 ## CARLA 0.9.15
 
diff --git a/Docs/build_linux.md b/Docs/build_linux.md
index e292a71d5..3be447d91 100644
--- a/Docs/build_linux.md
+++ b/Docs/build_linux.md
@@ -40,7 +40,7 @@ CARLA requires many different kinds of software to run. Some are built during th
 sudo apt-get update &&
 sudo apt-get install wget software-properties-common &&
 sudo add-apt-repository ppa:ubuntu-toolchain-r/test &&
-wget -O - https://apt.llvm.org/llvm-snapshot.gpg.key|sudo apt-key add
+sudo apt-get update
 ```
 
 !!! Warning
@@ -50,41 +50,28 @@ To avoid compatibility issues between Unreal Engine and the CARLA dependencies,
 
 __Ubuntu 22.04__.
 ```sh
-sudo apt-add-repository "deb http://archive.ubuntu.com/ubuntu focal main universe"
-sudo apt-get update
-sudo apt-get install build-essential clang-10 lld-10 g++-7 cmake ninja-build libvulkan1 python python3 python3-dev python3-pip libpng-dev libtiff5-dev libjpeg-dev tzdata sed curl unzip autoconf libtool rsync libxml2-dev git git-lfs
-sudo update-alternatives --install /usr/bin/clang++ clang++ /usr/lib/llvm-10/bin/clang++ 180 &&
-sudo update-alternatives --install /usr/bin/clang clang /usr/lib/llvm-10/bin/clang 180 &&
-sudo update-alternatives --install /usr/bin/g++ g++ /usr/bin/g++-7 180
+sudo apt-get install build-essential g++-13 cmake ninja-build libvulkan1 python3 python3-dev python3-pip libpng-dev libtiff5-dev libjpeg-dev tzdata sed curl unzip autoconf libtool rsync libxml2-dev git git-lfs
+sudo update-alternatives --install /usr/bin/g++ g++ /usr/bin/g++-13 180
 ```
 
 __Ubuntu 20.04__.
 ```sh
-sudo apt-add-repository "deb http://apt.llvm.org/focal/ llvm-toolchain-focal main"
-sudo apt-get update
-sudo apt-get install build-essential clang-10 lld-10 g++-7 cmake ninja-build libvulkan1 python python-dev python3-dev python3-pip libpng-dev libtiff5-dev libjpeg-dev tzdata sed curl unzip autoconf libtool rsync libxml2-dev git
-sudo update-alternatives --install /usr/bin/clang++ clang++ /usr/lib/llvm-10/bin/clang++ 180 &&
-sudo update-alternatives --install /usr/bin/clang clang /usr/lib/llvm-10/bin/clang 180
+sudo apt-get install build-essential g++-13 cmake ninja-build libvulkan1 python python-dev python3-dev python3-pip libpng-dev libtiff5-dev libjpeg-dev tzdata sed curl unzip autoconf libtool rsync libxml2-dev git
+sudo update-alternatives --install /usr/bin/g++ g++ /usr/bin/g++-13 180
 ```
 
 __Ubuntu 18.04__.
 
 ```sh
-sudo apt-add-repository "deb http://apt.llvm.org/bionic/ llvm-toolchain-bionic main"
-sudo apt-get update
-sudo apt-get install build-essential clang-8 lld-8 g++-7 cmake ninja-build libvulkan1 python python-pip python-dev python3-dev python3-pip libpng-dev libtiff5-dev libjpeg-dev tzdata sed curl unzip autoconf libtool rsync libxml2-dev git
-sudo update-alternatives --install /usr/bin/clang++ clang++ /usr/lib/llvm-8/bin/clang++ 180 &&
-sudo update-alternatives --install /usr/bin/clang clang /usr/lib/llvm-8/bin/clang 180
+sudo apt-get install build-essential g++-13 cmake ninja-build libvulkan1 python python-pip python-dev python3-dev python3-pip libpng-dev libtiff5-dev libjpeg-dev tzdata sed curl unzip autoconf libtool rsync libxml2-dev git
+sudo update-alternatives --install /usr/bin/g++ g++ /usr/bin/g++-13 180
 ```
 
 __Ubuntu 16.04__.
 
 ```sh
-sudo apt-add-repository "deb http://apt.llvm.org/xenial/ llvm-toolchain-xenial-8 main" &&
-sudo apt-get update
-sudo apt-get install build-essential clang-8 lld-8 g++-7 cmake ninja-build libvulkan1 python python-pip python-dev python3-dev python3-pip libpng16-dev libtiff5-dev libjpeg-dev tzdata sed curl unzip autoconf libtool rsync libxml2-dev git
-sudo update-alternatives --install /usr/bin/clang++ clang++ /usr/lib/llvm-8/bin/clang++ 180 &&
-sudo update-alternatives --install /usr/bin/clang clang /usr/lib/llvm-8/bin/clang 180
+sudo apt-get install build-essential g++-13 cmake ninja-build libvulkan1 python python-pip python-dev python3-dev python3-pip libpng16-dev libtiff5-dev libjpeg-dev tzdata sed curl unzip autoconf libtool rsync libxml2-dev git
+sudo update-alternatives --install /usr/bin/g++ g++ /usr/bin/g++-13 180
 ```
 
 __Previous Ubuntu versions__.
@@ -92,11 +79,8 @@ __Previous Ubuntu versions__.
 We strongly advise using Ubuntu 18.04 or later to build CARLA. However, you may attempt to build CARLA on older versions of Ubuntu with the following commands:
 
 ```sh
-sudo apt-add-repository "deb http://apt.llvm.org/<code_name>/ llvm-toolchain-<code_name>-8 main" &&
-sudo apt-get update
-sudo apt-get install build-essential clang-8 lld-8 g++-7 cmake ninja-build libvulkan1 python python-pip python-dev python3-dev python3-pip libpng16-dev libtiff5-dev libjpeg-dev tzdata sed curl unzip autoconf libtool rsync libxml2-dev git
-sudo update-alternatives --install /usr/bin/clang++ clang++ /usr/lib/llvm-8/bin/clang++ 180 &&
-sudo update-alternatives --install /usr/bin/clang clang /usr/lib/llvm-8/bin/clang 180
+sudo apt-get install build-essential g++-13 cmake ninja-build libvulkan1 python python-pip python-dev python3-dev python3-pip libpng16-dev libtiff5-dev libjpeg-dev tzdata sed curl unzip autoconf libtool rsync libxml2-dev git
+sudo update-alternatives --install /usr/bin/g++ g++ /usr/bin/g++-13 180
 ```
 
 __All Ubuntu systems__.
diff --git a/Examples/CppClient/Makefile b/Examples/CppClient/Makefile
index 05713a94c..a2baa9104 100644
--- a/Examples/CppClient/Makefile
+++ b/Examples/CppClient/Makefile
@@ -4,8 +4,8 @@ BINDIR=$(CURDIR)/bin
 INSTALLDIR=$(CURDIR)/libcarla-install
 TOOLCHAIN=$(CURDIR)/ToolChain.cmake
 
-CC=/usr/bin/gcc-7
-CXX=/usr/bin/g++-7
+CC=/usr/bin/gcc-13
+CXX=/usr/bin/g++-13
 CXXFLAGS=-std=c++14 -pthread -fPIC -O3 -DNDEBUG -Werror -Wall -Wextra
 
 define log
diff --git a/LibCarla/cmake/CMakeLists.txt b/LibCarla/cmake/CMakeLists.txt
index d876d72b6..e1ede8f3d 100644
--- a/LibCarla/cmake/CMakeLists.txt
+++ b/LibCarla/cmake/CMakeLists.txt
@@ -4,10 +4,12 @@ project(libcarla)
 option(LIBCARLA_BUILD_DEBUG "Build debug configuration" ON)
 option(LIBCARLA_BUILD_RELEASE "Build release configuration" ON)
 option(LIBCARLA_BUILD_TEST "Build unit tests" ON)
+option(LIBCARLA_USE_ROS "Build ROS variant" OFF)
 
 message(STATUS "Build debug:   ${LIBCARLA_BUILD_DEBUG}")
 message(STATUS "Build release: ${LIBCARLA_BUILD_RELEASE}")
 message(STATUS "Build test:    ${LIBCARLA_BUILD_TEST}")
+message(STATUS "Enable ROS:    ${LIBCARLA_USE_ROS}")
 
 set(libcarla_source_path "${PROJECT_SOURCE_DIR}/../source")
 set(libcarla_source_thirdparty_path "${libcarla_source_path}/third-party")
@@ -24,14 +26,12 @@ if (CMAKE_BUILD_TYPE STREQUAL "Client")
 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")
+  add_subdirectory("pytorch")
 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")) AND (NOT (CMAKE_BUILD_TYPE STREQUAL "ros2")))
+if ((LIBCARLA_BUILD_TEST) AND (NOT WIN32) AND (NOT (CMAKE_BUILD_TYPE STREQUAL "Pytorch")))
   add_subdirectory("test")
 endif()
diff --git a/LibCarla/cmake/client/CMakeLists.txt b/LibCarla/cmake/client/CMakeLists.txt
index 20438ad3e..b12870875 100644
--- a/LibCarla/cmake/client/CMakeLists.txt
+++ b/LibCarla/cmake/client/CMakeLists.txt
@@ -21,8 +21,8 @@ if (BUILD_RSS_VARIANT)
   install(FILES ${spdlog_file} DESTINATION lib)
   list(APPEND ADRSS_LIBS ${spdlog_file})
 
-  set(proj_include_dir ${ADRSS_INSTALL_DIR}/../../proj-install/include)
-  set(proj_lib ${ADRSS_INSTALL_DIR}/../../proj-install/lib/libproj.a)
+  set(proj_include_dir ${PROJ_INCLUDE_PATH})
+  set(proj_lib ${PROJ_LIBPATH}/libproj.a)
   install(DIRECTORY ${proj_include_dir} DESTINATION include/system)
   list(APPEND ADRSS_INCLUDE_DIRS ${proj_include_dir})
   install(FILES ${proj_lib} DESTINATION lib)
@@ -98,6 +98,12 @@ file(GLOB libcarla_carla_sources
 set(libcarla_sources "${libcarla_sources};${libcarla_carla_sources}")
 install(FILES ${libcarla_carla_sources} DESTINATION include/carla)
 
+file(GLOB libcarla_carla_actors_sources
+    "${libcarla_source_path}/carla/actors/*.cpp"
+    "${libcarla_source_path}/carla/actors/*.h")
+set(libcarla_sources "${libcarla_sources};${libcarla_carla_actors_sources}")
+install(FILES ${libcarla_carla_actors_sources} DESTINATION include/carla/actors)
+
 file(GLOB libcarla_carla_client_sources
     "${libcarla_source_path}/carla/client/*.cpp"
     "${libcarla_source_path}/carla/client/*.h")
@@ -266,11 +272,10 @@ 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)
+  file(GLOB libcarla_carla_ros2_public_headers
+    "${libcarla_source_path}/carla/ros2/*.h"
+  )
+  install(FILES ${libcarla_carla_ros2_public_headers} DESTINATION include/carla/ros2)
 
 # ==============================================================================
 # Create targets for debug and release in the same build type.
diff --git a/LibCarla/cmake/fast_dds/CMakeLists.txt b/LibCarla/cmake/fast_dds/CMakeLists.txt
deleted file mode 100644
index 9f2bd0378..000000000
--- a/LibCarla/cmake/fast_dds/CMakeLists.txt
+++ /dev/null
@@ -1,56 +0,0 @@
-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()
diff --git a/LibCarla/cmake/server/CMakeLists.txt b/LibCarla/cmake/server/CMakeLists.txt
index 5c642dd49..3ee530379 100644
--- a/LibCarla/cmake/server/CMakeLists.txt
+++ b/LibCarla/cmake/server/CMakeLists.txt
@@ -1,78 +1,51 @@
 cmake_minimum_required(VERSION 3.5.1)
 project(libcarla-server)
 
-# Install libc++ shared libraries.
-file(GLOB LibCXX_Libraries "${LLVM_LIB_PATH}/libc++*")
-install(FILES ${LibCXX_Libraries} DESTINATION lib)
-
-# Install rpclib.
-install(DIRECTORY "${RPCLIB_INCLUDE_PATH}/rpc" DESTINATION include)
-file(GLOB libcarla_carla_rpclib "${RPCLIB_LIB_PATH}/*.*")
-install(FILES ${libcarla_carla_rpclib} DESTINATION lib)
-
-# Install headers.
-
 install(DIRECTORY "${libcarla_source_path}/compiler" DESTINATION include)
 
-file(GLOB libcarla_carla_headers "${libcarla_source_path}/carla/*.h")
-install(FILES ${libcarla_carla_headers} DESTINATION include/carla)
+foreach(dir "" 
+            "actors/"
+            "geom/"
+            "opendrive/" "opendrive/parser/"
+            "profiler/"
+            "road/" "road/element/" "road/general/" "road/object/" "road/signal/"
+            "rpc/"
+            "sensor/" "sensor/data/" "sensor/s11n/"
+            "streaming/" "streaming/detail/" "streaming/detail/tcp/" "streaming/low_level/"
+            "multigpu/")
 
-file(GLOB libcarla_carla_geom_headers "${libcarla_source_path}/carla/geom/*.h")
-install(FILES ${libcarla_carla_geom_headers} DESTINATION include/carla/geom)
+  file(GLOB headers "${libcarla_source_path}/carla/${dir}*.h")
+  install(FILES ${headers} DESTINATION include/carla/${dir})
 
-file(GLOB libcarla_carla_opendrive "${libcarla_source_path}/carla/opendrive/*.h")
-install(FILES ${libcarla_carla_opendrive} DESTINATION include/carla/opendrive)
+endforeach()
 
-file(GLOB libcarla_carla_opendrive_parser "${libcarla_source_path}/carla/opendrive/parser/*.h")
-install(FILES ${libcarla_carla_opendrive_parser} DESTINATION include/carla/opendrive/parser)
 
-file(GLOB libcarla_carla_profiler_headers "${libcarla_source_path}/carla/profiler/*.h")
-install(FILES ${libcarla_carla_profiler_headers} DESTINATION include/carla/profiler)
 
-file(GLOB libcarla_carla_road_headers "${libcarla_source_path}/carla/road/*.h")
-install(FILES ${libcarla_carla_road_headers} DESTINATION include/carla/road)
+  # only install the required public interface headers
+  foreach(dir "" "types/" )
+    file(GLOB libcarla_carla_ros2_public_headers
+      "${libcarla_source_path}/carla/ros2/${dir}*.h"
+    )
+    install(FILES ${libcarla_carla_ros2_public_headers} DESTINATION include/carla/ros2/${dir})
+  endforeach()
+if (LIBCARLA_USE_ROS)
 
-file(GLOB libcarla_carla_road_element_headers "${libcarla_source_path}/carla/road/element/*.h")
-install(FILES ${libcarla_carla_road_element_headers} DESTINATION include/carla/road/element)
+  file(GLOB subdirs RELATIVE "${libcarla_source_path}/carla/ros2/${ROS2_MW_NAME}" "${libcarla_source_path}/carla/ros2/${ROS2_MW_NAME}/*")
+  foreach(typedir "msg" "srv")
+    foreach(dir ${subdirs})
+      if(IS_DIRECTORY "${libcarla_source_path}/carla/ros2/${ROS2_MW_NAME}/${dir}/${typedir}")
+        file(GLOB libcarla_carla_ros2_types_${dir}_headers
+              "${libcarla_source_path}/carla/ros2/${ROS2_MW_NAME}/${dir}/${typedir}/*.h"
+        )
+        install(FILES ${libcarla_carla_ros2_types_${dir}_headers} DESTINATION include/carla/ros2/ros_types/${dir}/${typedir}/)
+      endif()
+    endforeach()
+  endforeach()
 
-file(GLOB libcarla_carla_road_general_headers "${libcarla_source_path}/carla/road/general/*.h")
-install(FILES ${libcarla_carla_road_general_headers} DESTINATION include/carla/road/general)
-
-file(GLOB libcarla_carla_road_object_headers "${libcarla_source_path}/carla/road/object/*.h")
-install(FILES ${libcarla_carla_road_object_headers} DESTINATION include/carla/road/object)
-
-file(GLOB libcarla_carla_road_signal_headers "${libcarla_source_path}/carla/road/signal/*.h")
-install(FILES ${libcarla_carla_road_signal_headers} DESTINATION include/carla/road/signal)
-
-file(GLOB libcarla_carla_rpc_headers "${libcarla_source_path}/carla/rpc/*.h")
-install(FILES ${libcarla_carla_rpc_headers} DESTINATION include/carla/rpc)
-
-file(GLOB libcarla_carla_sensor_headers "${libcarla_source_path}/carla/sensor/*.h")
-install(FILES ${libcarla_carla_sensor_headers} DESTINATION include/carla/sensor)
-
-file(GLOB libcarla_carla_sensor_data_headers "${libcarla_source_path}/carla/sensor/data/*.h")
-install(FILES ${libcarla_carla_sensor_data_headers} DESTINATION include/carla/sensor/data)
-
-file(GLOB libcarla_carla_sensor_s11n_headers "${libcarla_source_path}/carla/sensor/s11n/*.h")
-install(FILES ${libcarla_carla_sensor_s11n_headers} DESTINATION include/carla/sensor/s11n)
-
-file(GLOB libcarla_carla_streaming_headers "${libcarla_source_path}/carla/streaming/*.h")
-install(FILES ${libcarla_carla_streaming_headers} DESTINATION include/carla/streaming)
-
-file(GLOB libcarla_carla_streaming_detail_headers "${libcarla_source_path}/carla/streaming/detail/*.h")
-install(FILES ${libcarla_carla_streaming_detail_headers} DESTINATION include/carla/streaming/detail)
-
-file(GLOB libcarla_carla_streaming_detail_tcp_headers "${libcarla_source_path}/carla/streaming/detail/tcp/*.h")
-install(FILES ${libcarla_carla_streaming_detail_tcp_headers} DESTINATION include/carla/streaming/detail/tcp)
-
-file(GLOB libcarla_carla_streaming_low_level_headers "${libcarla_source_path}/carla/streaming/low_level/*.h")
-install(FILES ${libcarla_carla_streaming_low_level_headers} DESTINATION include/carla/streaming/low_level)
-
-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)
+  if(NOT WIN32)
+    set(CMAKE_CXX_FLAGS_RELEASE "${CMAKE_CXX_FLAGS_RELEASE} -O3" CACHE STRING "" FORCE)
+  endif()
+endif()
 
 install(DIRECTORY "${BOOST_INCLUDE_PATH}/boost" DESTINATION include)
 
@@ -85,47 +58,62 @@ if(WIN32)
 endif()
 
 # carla_server library.
+# first the sources which are picked from some directories
+set(libcarla_server_sources
+  "${libcarla_source_path}/carla/Buffer.cpp"
+  "${libcarla_source_path}/carla/Exception.cpp"
+  "${libcarla_source_path}/carla/StringUtil.cpp"
+  # other cpp files only define Deserialize used in client
+  "${libcarla_source_path}/carla/sensor/s11n/SensorHeaderSerializer.cpp"
+  )
+# then the globs on the folders to add as a whole
+foreach(dir 
+      "actors"
+      "geom"
+      "opendrive" "opendrive/parser" 
+      "profiler"
+      "road" "road/element" "road/general" "road/object" "road/signal"
+      "rpc"
+      "sensor" "sensor/data"
+      "streaming" "streaming/detail" "streaming/detail/tcp" "streaming/low_level"
+      "multigpu")
 
-file(GLOB libcarla_server_sources
-    "${libcarla_source_path}/carla/*.h"
-    "${libcarla_source_path}/carla/Buffer.cpp"
-    "${libcarla_source_path}/carla/Exception.cpp"
-    "${libcarla_source_path}/carla/geom/*.cpp"
-    "${libcarla_source_path}/carla/geom/*.h"
-    "${libcarla_source_path}/carla/opendrive/*.cpp"
-    "${libcarla_source_path}/carla/opendrive/*.h"
-    "${libcarla_source_path}/carla/opendrive/parser/*.cpp"
-    "${libcarla_source_path}/carla/opendrive/parser/*.h"
-    "${libcarla_source_path}/carla/road/*.cpp"
-    "${libcarla_source_path}/carla/road/*.h"
-    "${libcarla_source_path}/carla/road/element/*.cpp"
-    "${libcarla_source_path}/carla/road/element/*.h"
-    "${libcarla_source_path}/carla/road/general/*.cpp"
-    "${libcarla_source_path}/carla/road/general/*.h"
-    "${libcarla_source_path}/carla/road/object/*.cpp"
-    "${libcarla_source_path}/carla/road/object/*.h"
-    "${libcarla_source_path}/carla/road/signal/*.cpp"
-    "${libcarla_source_path}/carla/road/signal/*.h"
-    "${libcarla_source_path}/carla/rpc/*.cpp"
-    "${libcarla_source_path}/carla/rpc/*.h"
-    "${libcarla_source_path}/carla/sensor/*.h"
-    "${libcarla_source_path}/carla/sensor/s11n/*.h"
-    "${libcarla_source_path}/carla/sensor/s11n/SensorHeaderSerializer.cpp"
-    "${libcarla_source_path}/carla/streaming/*.h"
-    "${libcarla_source_path}/carla/streaming/detail/*.cpp"
-    "${libcarla_source_path}/carla/streaming/detail/*.h"
-    "${libcarla_source_path}/carla/streaming/detail/tcp/*.cpp"
-    "${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_thirdparty_path}/moodycamel/*.h"
-    "${libcarla_source_thirdparty_path}/pugixml/*.cpp"
-    "${libcarla_source_thirdparty_path}/pugixml/*.hpp")
+  file(GLOB sources "${libcarla_source_path}/carla/${dir}/*.cpp")
+  list(APPEND libcarla_server_sources ${sources})
+
+endforeach()
+
+foreach(thirdparty_dir
+        "odrSpiral"
+        "moodycamel"
+        "pugixml")
+
+  file(GLOB sources "${libcarla_source_thirdparty_path}/${thirdparty_dir}/*.cpp")
+  list(APPEND libcarla_server_sources ${sources})
+
+endforeach()
+
+if (LIBCARLA_USE_ROS)
+  foreach(ros2_dir
+          "/"
+          "${ROS2_MW_NAME}/carla/ros2/impl/"
+          "listeners/"
+          "publishers/"
+          "services/"
+          "subscribers/"
+          "types/")
+
+    file(GLOB sources "${libcarla_source_path}/carla/ros2/${ros2_dir}*.cpp")
+    list(APPEND libcarla_server_sources ${sources})
+  endforeach()
+
+  file(GLOB msg_sources "${libcarla_source_path}/carla/ros2/${ROS2_MW_NAME}/*/msg/*.cxx")
+  list(APPEND libcarla_server_sources ${msg_sources})
+
+  file(GLOB srv_sources "${libcarla_source_path}/carla/ros2/${ROS2_MW_NAME}/*/srv/*.cxx")
+  list(APPEND libcarla_server_sources ${srv_sources})
+
+endif()
 
 # ==============================================================================
 # Create targets for debug and release in the same build type.
@@ -133,7 +121,10 @@ file(GLOB libcarla_server_sources
 
 if (LIBCARLA_BUILD_RELEASE)
 
-  add_library(carla_server STATIC ${libcarla_server_sources})
+  add_library(carla_server STATIC
+    ${libcarla_server_sources}
+  )
+  target_compile_definitions(carla_server PUBLIC -DCARLA_SERVER_BUILD)
 
   target_include_directories(carla_server SYSTEM PRIVATE
       "${BOOST_INCLUDE_PATH}"
@@ -143,11 +134,23 @@ if (LIBCARLA_BUILD_RELEASE)
 
   set_target_properties(carla_server PROPERTIES COMPILE_FLAGS "${CMAKE_CXX_FLAGS_RELEASE}")
 
+  if (LIBCARLA_USE_ROS)
+    target_include_directories(carla_server SYSTEM PRIVATE
+     # first the ROS2_MW_NAME local folder allowing potential overrides of header files
+     "${libcarla_source_path}/carla/ros2/${ROS2_MW_NAME}"
+     "${ROS2_MW_INCLUDE_PATH}")
+    target_link_directories(carla_server PRIVATE ${ROS2_MW_LIB_PATH})
+    target_link_libraries(carla_server ${ROS2_MW_LINK_LIBRARIES})
+    target_compile_definitions(carla_server PUBLIC -DWITH_ROS2)
+  endif()
 endif()
 
 if (LIBCARLA_BUILD_DEBUG)
 
-  add_library(carla_server_debug STATIC ${libcarla_server_sources})
+  add_library(carla_server_debug STATIC
+    ${libcarla_server_sources}
+  )
+  target_compile_definitions(carla_server_debug PUBLIC -DCARLA_SERVER_BUILD)
 
   target_include_directories(carla_server_debug SYSTEM PRIVATE
       "${BOOST_INCLUDE_PATH}"
@@ -158,4 +161,13 @@ if (LIBCARLA_BUILD_DEBUG)
   set_target_properties(carla_server_debug PROPERTIES COMPILE_FLAGS "${CMAKE_CXX_FLAGS_DEBUG}")
   target_compile_definitions(carla_server_debug PUBLIC -DBOOST_ASIO_ENABLE_BUFFER_DEBUGGING)
 
+  if (LIBCARLA_USE_ROS)
+    target_include_directories(carla_server_debug SYSTEM PRIVATE
+     # first the ROS2_MW_NAME local folder allowing potential overrides of header files
+     "${libcarla_source_path}/carla/ros2/${ROS2_MW_NAME}"
+     "${ROS2_MW_INCLUDE_PATH}")
+     target_link_directories(carla_server_debug PRIVATE ${ROS2_MW_LIB_PATH})
+     target_link_libraries(carla_server_debug ${ROS2_MW_LINK_LIBRARIES})
+     target_compile_definitions(carla_server_debug PUBLIC -DWITH_ROS2)
+     endif()
 endif()
diff --git a/LibCarla/cmake/test/CMakeLists.txt b/LibCarla/cmake/test/CMakeLists.txt
index 432f509e7..23eefc167 100644
--- a/LibCarla/cmake/test/CMakeLists.txt
+++ b/LibCarla/cmake/test/CMakeLists.txt
@@ -15,6 +15,10 @@ endif()
 link_directories(
     ${RPCLIB_LIB_PATH}
     ${GTEST_LIB_PATH})
+if (LIBCARLA_USE_ROS)
+  link_directories(${ROS2_MW_LIB_PATH})
+endif()
+
 
 file(GLOB libcarla_test_sources
     "${libcarla_source_path}/carla/profiler/*.cpp"
diff --git a/LibCarla/source/carla/Exception.cpp b/LibCarla/source/carla/Exception.cpp
index 1ed5c7965..3dfd7ec8a 100644
--- a/LibCarla/source/carla/Exception.cpp
+++ b/LibCarla/source/carla/Exception.cpp
@@ -15,19 +15,18 @@
 
 namespace boost {
 
-  void throw_exception(const std::exception &e) {
-    carla::throw_exception(e);
-  }
+void throw_exception(const std::exception &e) {
+  carla::throw_exception(e);
+}
 
-  void throw_exception(
-      const std::exception &e,
-      boost::source_location const & loc) {
-    throw_exception(e);
-  }
+void throw_exception(const std::exception &e, boost::source_location const &loc) {
+  (void)loc;
+  throw_exception(e);
+}
 
-} // namespace boost
+}  // namespace boost
 
-#endif // BOOST_NO_EXCEPTIONS
+#endif  // BOOST_NO_EXCEPTIONS
 
 // =============================================================================
 // -- Workaround for Boost.Asio bundled with rpclib ----------------------------
@@ -42,16 +41,47 @@ namespace boost {
 namespace clmdep_asio {
 namespace detail {
 
-  template <typename Exception>
-  void throw_exception(const Exception& e) {
-    carla::throw_exception(e);
-  }
+template <typename Exception>
+void throw_exception(const Exception &e) {
+  carla::throw_exception(e);
+}
 
-  template void throw_exception<std::bad_cast>(const std::bad_cast &);
-  template void throw_exception<std::exception>(const std::exception &);
-  template void throw_exception<std::system_error>(const std::system_error &);
+template void throw_exception<std::bad_cast>(const std::bad_cast &e);
+template void throw_exception<std::exception>(const std::exception &e);
+template void throw_exception<std::system_error>(const std::system_error &e);
 
-} // namespace detail
-} // namespace clmdep_asio
+}  // namespace detail
+}  // namespace clmdep_asio
 
-#endif // ASIO_NO_EXCEPTIONS
+namespace asio {
+namespace detail {
+
+template <typename Exception>
+void throw_exception(const Exception &e) {
+  carla::throw_exception(e);
+}
+
+template void throw_exception<std::bad_cast>(const std::bad_cast &e);
+template void throw_exception<std::exception>(const std::exception &e);
+template void throw_exception<std::system_error>(const std::system_error &e);
+
+}  // namespace detail
+}  // namespace asio
+
+#endif  // ASIO_NO_EXCEPTIONS
+
+#ifndef LIBCARLA_NO_EXCEPTIONS
+
+namespace carla {
+template
+#ifndef __clang__
+    // clang doesn't support C++11 attributes in template explicit instantiation, since attributes in each case cannot
+    // be changed here MSVC requires it (might be a bit too conservative and requires the attributes also in explicit
+    // instanciation)
+    [[noreturn]]
+#endif
+    void
+    throw_exception<std::exception>(const std::exception &);
+}  // namespace carla
+
+#endif
diff --git a/LibCarla/source/carla/Exception.h b/LibCarla/source/carla/Exception.h
index 6df36e086..843294a29 100644
--- a/LibCarla/source/carla/Exception.h
+++ b/LibCarla/source/carla/Exception.h
@@ -6,38 +6,34 @@
 
 #pragma once
 
+#include <exception>
+
 #ifdef LIBCARLA_NO_EXCEPTIONS
 
-namespace std {
-
-  class exception;
-
-} // namespace std
-
 namespace carla {
 
-  /// User define function, similar to Boost throw_exception.
-  ///
-  /// @important Boost exceptions are also routed to this function.
-  ///
-  /// When compiled with LIBCARLA_NO_EXCEPTIONS, this function is left undefined
-  /// in LibCarla, and the modules using LibCarla are expected to supply an
-  /// appropriate definition. Callers of throw_exception are allowed to assume
-  /// that the function never returns; therefore, if the user-defined
-  /// throw_exception returns, the behavior is undefined.
-  [[ noreturn ]] void throw_exception(const std::exception &e);
+/// User define function, similar to Boost throw_exception.
+///
+/// @important Boost exceptions are also routed to this function.
+///
+/// When compiled with LIBCARLA_NO_EXCEPTIONS, this function is left undefined
+/// in LibCarla, and the modules using LibCarla are expected to supply an
+/// appropriate definition. Callers of throw_exception are allowed to assume
+/// that the function never returns; therefore, if the user-defined
+/// throw_exception returns, the behavior is undefined.
+[[noreturn]] void throw_exception(const std::exception &e);
 
-} // namespace carla
+}  // namespace carla
 
 #else
 
 namespace carla {
 
-  template <typename T>
-  [[ noreturn ]] void throw_exception(const T &e) {
-    throw e;
-  }
+template <typename T>
+[[noreturn]] void throw_exception(const T &e) {
+  throw e;
+}
 
-} // namespace carla
+}  // namespace carla
 
-#endif // LIBCARLA_NO_EXCEPTIONS
+#endif  // LIBCARLA_NO_EXCEPTIONS
diff --git a/LibCarla/source/carla/geom/Location.h b/LibCarla/source/carla/geom/Location.h
index 1ce3fcecf..9e86a15e5 100644
--- a/LibCarla/source/carla/geom/Location.h
+++ b/LibCarla/source/carla/geom/Location.h
@@ -54,7 +54,7 @@ namespace geom {
     // =========================================================================
 
     Location &operator+=(const Location &rhs) {
-      static_cast<Vector3D &>(*this) += rhs;
+      static_cast<Vector3D &>(*this) += static_cast<const Vector3D &>(rhs);
       return *this;
     }
 
@@ -64,7 +64,7 @@ namespace geom {
     }
 
     Location &operator-=(const Location &rhs) {
-      static_cast<Vector3D &>(*this) -= rhs;
+      static_cast<Vector3D &>(*this) -= static_cast<const Vector3D &>(rhs);
       return *this;
     }
 
@@ -78,7 +78,7 @@ namespace geom {
     // =========================================================================
 
     bool operator==(const Location &rhs) const {
-      return static_cast<const Vector3D &>(*this) == rhs;
+      return static_cast<const Vector3D &>(*this) == static_cast<const Vector3D &>(rhs);
     }
 
     bool operator!=(const Location &rhs) const {
diff --git a/LibCarla/source/carla/ros2/Exception.cpp b/LibCarla/source/carla/ros2/Exception.cpp
new file mode 100644
index 000000000..58f298c82
--- /dev/null
+++ b/LibCarla/source/carla/ros2/Exception.cpp
@@ -0,0 +1,30 @@
+// Copyright (c) 2017 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/Exception.h"
+#include <asio.hpp>
+
+#ifdef ASIO_NO_EXCEPTIONS
+
+namespace asio {
+namespace detail {
+
+template <typename Exception>
+void throw_exception(const Exception &e) {
+  carla::throw_exception(e);
+}
+
+template void throw_exception<std::length_error>(const std::length_error &e);
+template void throw_exception<asio::service_already_exists>(const asio::service_already_exists &e);
+template void throw_exception<asio::invalid_service_owner>(const asio::invalid_service_owner &e);
+template void throw_exception<asio::execution::bad_executor>(const asio::execution::bad_executor &e);
+template void throw_exception<asio::ip::bad_address_cast>(const asio::ip::bad_address_cast &e);
+template void throw_exception<std::out_of_range>(const std::out_of_range &e);
+
+}  // namespace detail
+}  // namespace asio
+
+#endif  // ASIO_NO_EXCEPTIONS
diff --git a/LibCarla/source/carla/ros2/ROS2.cpp b/LibCarla/source/carla/ros2/ROS2.cpp
index 719e66237..63536aacb 100644
--- a/LibCarla/source/carla/ros2/ROS2.cpp
+++ b/LibCarla/source/carla/ros2/ROS2.cpp
@@ -16,6 +16,7 @@
 #include "carla/sensor/s11n/ImageSerializer.h"
 #include "carla/sensor/s11n/SensorHeaderSerializer.h"
 
+#include "fastdds/carla/ros2/impl/DdsDomainParticipantImpl.h"
 #include "publishers/CarlaPublisher.h"
 #include "publishers/CarlaClockPublisher.h"
 #include "publishers/CarlaRGBCameraPublisher.h"
@@ -46,6 +47,14 @@ namespace ros2 {
 
 // static fields
 std::shared_ptr<ROS2> ROS2::_instance;
+std::shared_ptr<DdsDomainParticipantImpl> ROS2::GetDdsDomainParticipant() {
+  auto _instance = ROS2::GetInstance();
+  if (_instance->_domain_participant_impl == nullptr) 
+  {
+    _instance->_domain_participant_impl = std::make_shared<DdsDomainParticipantImpl>();
+  }
+  return _instance->_domain_participant_impl;
+}
 
 // list of sensors (should be equal to the list of SensorsRegistry
 enum ESensors {
@@ -841,6 +850,9 @@ void ROS2::Shutdown() {
   _clock_publisher.reset();
   _controller.reset();
   _enabled = false;
+  // finally, release the rest of our resources by destroying our instance
+  // to prevent from race conditions on system level shutdown of global resources
+  _instance = nullptr;
 }
 
 } // namespace ros2
diff --git a/LibCarla/source/carla/ros2/ROS2.h b/LibCarla/source/carla/ros2/ROS2.h
index 8955d3d14..4de19d7a2 100644
--- a/LibCarla/source/carla/ros2/ROS2.h
+++ b/LibCarla/source/carla/ros2/ROS2.h
@@ -41,6 +41,7 @@ namespace ros2 {
   class CarlaTransformPublisher;
   class CarlaClockPublisher;
   class CarlaEgoVehicleControlSubscriber;
+  class DdsDomainParticipantImpl;
 
 class ROS2
 {
@@ -54,6 +55,8 @@ class ROS2
     return _instance;
   }
 
+  static std::shared_ptr<DdsDomainParticipantImpl> GetDdsDomainParticipant();
+
   // general
   void Enable(bool enable);
   void Shutdown();
@@ -148,6 +151,7 @@ void ProcessDataFromCollisionSensor(
   ROS2() {};
 
   static std::shared_ptr<ROS2> _instance;
+  std::shared_ptr<DdsDomainParticipantImpl> _domain_participant_impl;
 
   bool _enabled { false };
   uint64_t _frame { 0 };
diff --git a/LibCarla/source/carla/ros2/fastdds/carla/ros2/impl/DdsDomainParticipantImpl.cpp b/LibCarla/source/carla/ros2/fastdds/carla/ros2/impl/DdsDomainParticipantImpl.cpp
new file mode 100644
index 000000000..b498e27cc
--- /dev/null
+++ b/LibCarla/source/carla/ros2/fastdds/carla/ros2/impl/DdsDomainParticipantImpl.cpp
@@ -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>.
+
+#include "carla/ros2/impl/DdsDomainParticipantImpl.h"
+
+#include <cstdlib>
+#include <string>
+
+#include <fastdds/dds/domain/DomainParticipantFactory.hpp>
+#include <fastdds/dds/domain/qos/DomainParticipantQos.hpp>
+
+#include "carla/Logging.h"
+
+namespace carla {
+namespace ros2 {
+
+DdsDomainParticipantImpl::DdsDomainParticipantImpl() {
+  _factory = eprosima::fastdds::dds::DomainParticipantFactory::get_shared_instance();
+  if (_factory == nullptr) {
+    carla::log_error("DdsDomainParticipantImpl(): Failed to acquire DomainParticipantFactory");
+    return;
+  }
+
+  const char *ros_domain_id_env = std::getenv("ROS_DOMAIN_ID");
+  unsigned int ros_domain_id = 0;
+  if ( ros_domain_id_env != nullptr ) {
+    try {
+      ros_domain_id = (unsigned int)(std::atoi(ros_domain_id_env));
+    } catch (...) {
+      ros_domain_id = 0;
+    }
+  }
+  auto pqos = eprosima::fastdds::dds::PARTICIPANT_QOS_DEFAULT;
+  pqos.name("carla-server");
+  _participant = _factory->create_participant(ros_domain_id, pqos);
+  if (_participant == nullptr) {
+    carla::log_error("DdsDomainParticipantImpl(): Failed to create DomainParticipant");
+  }
+}
+
+DdsDomainParticipantImpl::~DdsDomainParticipantImpl() {
+  carla::log_warning("DdsDomainParticipantImpl::Destructor()");
+  if ((_participant != nullptr) && (_factory != nullptr)) {
+    _factory->delete_participant(_participant);
+    _participant=nullptr;
+  }
+}
+
+}  // namespace ros2
+}  // namespace carla
diff --git a/LibCarla/source/carla/ros2/fastdds/carla/ros2/impl/DdsDomainParticipantImpl.h b/LibCarla/source/carla/ros2/fastdds/carla/ros2/impl/DdsDomainParticipantImpl.h
new file mode 100644
index 000000000..1b8f7612a
--- /dev/null
+++ b/LibCarla/source/carla/ros2/fastdds/carla/ros2/impl/DdsDomainParticipantImpl.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
+
+#include <fastdds/dds/domain/DomainParticipant.hpp>
+#include <fastdds/dds/domain/DomainParticipantFactory.hpp>
+
+namespace carla {
+namespace ros2 {
+
+class DdsDomainParticipantImpl {
+public:
+  DdsDomainParticipantImpl();
+  ~DdsDomainParticipantImpl();
+
+  eprosima::fastdds::dds::DomainParticipant* GetDomainParticipant() {
+    return _participant;
+  }
+
+private:
+  eprosima::fastdds::dds::DomainParticipant* _participant{nullptr};
+  // keep also a copy of the factory that the underlying DDS is keeping their stuff up
+  std::shared_ptr<eprosima::fastdds::dds::DomainParticipantFactory> _factory;
+};
+
+}  // namespace ros2
+}  // namespace carla
\ No newline at end of file
diff --git a/LibCarla/source/carla/ros2/publishers/CarlaClockPublisher.cpp b/LibCarla/source/carla/ros2/publishers/CarlaClockPublisher.cpp
index b59e1f38d..2c195c614 100644
--- a/LibCarla/source/carla/ros2/publishers/CarlaClockPublisher.cpp
+++ b/LibCarla/source/carla/ros2/publishers/CarlaClockPublisher.cpp
@@ -13,8 +13,8 @@
 #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 <carla/ros2/ROS2.h>
+#include <carla/ros2/impl/DdsDomainParticipantImpl.h>
 #include <fastdds/dds/publisher/qos/PublisherQos.hpp>
 #include <fastdds/dds/topic/qos/TopicQos.hpp>
 
@@ -30,7 +30,6 @@ namespace ros2 {
   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 };
@@ -44,18 +43,12 @@ namespace ros2 {
         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);
+        auto participant = ROS2::GetDdsDomainParticipant()->GetDomainParticipant();;
+
+    _impl->_type.register_type(participant);
 
     efd::PublisherQos pubqos = efd::PUBLISHER_QOS_DEFAULT;
-    _impl->_publisher = _impl->_participant->create_publisher(pubqos, nullptr);
+    _impl->_publisher = participant->create_publisher(pubqos, nullptr);
     if (_impl->_publisher == nullptr) {
       std::cerr << "Failed to create Publisher" << std::endl;
       return false;
@@ -63,7 +56,7 @@ namespace ros2 {
 
     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);
+    _impl->_topic = participant->create_topic(topic_name, _impl->_type->getName(), tqos);
     if (_impl->_topic == nullptr) {
         std::cerr << "Failed to create Topic" << std::endl;
         return false;
@@ -157,17 +150,16 @@ namespace ros2 {
       if (!_impl)
           return;
 
+      auto participant = ROS2::GetDdsDomainParticipant()->GetDomainParticipant();
       if (_impl->_datawriter)
           _impl->_publisher->delete_datawriter(_impl->_datawriter);
 
       if (_impl->_publisher)
-          _impl->_participant->delete_publisher(_impl->_publisher);
+          participant->delete_publisher(_impl->_publisher);
 
       if (_impl->_topic)
-          _impl->_participant->delete_topic(_impl->_topic);
+          participant->delete_topic(_impl->_topic);
 
-      if (_impl->_participant)
-          efd::DomainParticipantFactory::get_instance()->delete_participant(_impl->_participant);
   }
 
   CarlaClockPublisher::CarlaClockPublisher(const CarlaClockPublisher& other) {
diff --git a/LibCarla/source/carla/ros2/publishers/CarlaCollisionPublisher.cpp b/LibCarla/source/carla/ros2/publishers/CarlaCollisionPublisher.cpp
index 2df478c60..4cb037deb 100644
--- a/LibCarla/source/carla/ros2/publishers/CarlaCollisionPublisher.cpp
+++ b/LibCarla/source/carla/ros2/publishers/CarlaCollisionPublisher.cpp
@@ -13,8 +13,8 @@
 #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 <carla/ros2/ROS2.h>
+#include <carla/ros2/impl/DdsDomainParticipantImpl.h>
 #include <fastdds/dds/publisher/qos/PublisherQos.hpp>
 #include <fastdds/dds/topic/qos/TopicQos.hpp>
 
@@ -31,7 +31,6 @@ namespace ros2 {
   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 };
@@ -46,18 +45,12 @@ namespace ros2 {
         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);
+    auto participant = ROS2::GetDdsDomainParticipant()->GetDomainParticipant();
+
+    _impl->_type.register_type(participant);
 
     efd::PublisherQos pubqos = efd::PUBLISHER_QOS_DEFAULT;
-    _impl->_publisher = _impl->_participant->create_publisher(pubqos, nullptr);
+    _impl->_publisher = participant->create_publisher(pubqos, nullptr);
     if (_impl->_publisher == nullptr) {
       std::cerr << "Failed to create Publisher" << std::endl;
       return false;
@@ -69,7 +62,7 @@ namespace ros2 {
     if (!_parent.empty())
       topic_name += _parent + "/";
     topic_name += _name;
-    _impl->_topic = _impl->_participant->create_topic(topic_name, _impl->_type->getName(), tqos);
+    _impl->_topic = participant->create_topic(topic_name, _impl->_type->getName(), tqos);
     if (_impl->_topic == nullptr) {
         std::cerr << "Failed to create Topic" << std::endl;
         return false;
@@ -183,17 +176,16 @@ void CarlaCollisionPublisher::SetData(int32_t seconds, uint32_t nanoseconds, uin
       if (!_impl)
           return;
 
+      auto participant = ROS2::GetDdsDomainParticipant()->GetDomainParticipant();
       if (_impl->_datawriter)
           _impl->_publisher->delete_datawriter(_impl->_datawriter);
 
       if (_impl->_publisher)
-          _impl->_participant->delete_publisher(_impl->_publisher);
+          participant->delete_publisher(_impl->_publisher);
 
       if (_impl->_topic)
-          _impl->_participant->delete_topic(_impl->_topic);
+          participant->delete_topic(_impl->_topic);
 
-      if (_impl->_participant)
-          efd::DomainParticipantFactory::get_instance()->delete_participant(_impl->_participant);
   }
 
   CarlaCollisionPublisher::CarlaCollisionPublisher(const CarlaCollisionPublisher& other) {
diff --git a/LibCarla/source/carla/ros2/publishers/CarlaDVSCameraPublisher.cpp b/LibCarla/source/carla/ros2/publishers/CarlaDVSCameraPublisher.cpp
index 174df73e2..27b67662f 100644
--- a/LibCarla/source/carla/ros2/publishers/CarlaDVSCameraPublisher.cpp
+++ b/LibCarla/source/carla/ros2/publishers/CarlaDVSCameraPublisher.cpp
@@ -17,8 +17,8 @@
 #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 <carla/ros2/ROS2.h>
+#include <carla/ros2/impl/DdsDomainParticipantImpl.h>
 #include <fastdds/dds/publisher/qos/PublisherQos.hpp>
 #include <fastdds/dds/topic/qos/TopicQos.hpp>
 
@@ -35,7 +35,6 @@ namespace ros2 {
   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 };
@@ -45,7 +44,6 @@ namespace ros2 {
   };
 
   struct CarlaCameraInfoPublisherImpl {
-    efd::DomainParticipant* _participant { nullptr };
     efd::Publisher* _publisher { nullptr };
     efd::Topic* _topic { nullptr };
     efd::DataWriter* _datawriter { nullptr };
@@ -56,7 +54,6 @@ namespace ros2 {
   };
 
   struct CarlaPointCloudPublisherImpl {
-    efd::DomainParticipant* _participant { nullptr };
     efd::Publisher* _publisher { nullptr };
     efd::Topic* _topic { nullptr };
     efd::DataWriter* _datawriter { nullptr };
@@ -85,18 +82,12 @@ namespace ros2 {
         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);
+    auto participant = ROS2::GetDdsDomainParticipant()->GetDomainParticipant();
+
+    _impl->_type.register_type(participant);
 
     efd::PublisherQos pubqos = efd::PUBLISHER_QOS_DEFAULT;
-    _impl->_publisher = _impl->_participant->create_publisher(pubqos, nullptr);
+    _impl->_publisher = participant->create_publisher(pubqos, nullptr);
     if (_impl->_publisher == nullptr) {
       std::cerr << "Failed to create Publisher" << std::endl;
       return false;
@@ -110,7 +101,7 @@ namespace ros2 {
       topic_name += _parent + "/";
     topic_name += _name;
     topic_name += publisher_type;
-    _impl->_topic = _impl->_participant->create_topic(topic_name, _impl->_type->getName(), tqos);
+    _impl->_topic = participant->create_topic(topic_name, _impl->_type->getName(), tqos);
     if (_impl->_topic == nullptr) {
         std::cerr << "Failed to create Topic" << std::endl;
         return false;
@@ -134,18 +125,11 @@ namespace ros2 {
         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);
+    auto participant = ROS2::GetDdsDomainParticipant()->GetDomainParticipant();
+    _info->_type.register_type(participant);
 
     efd::PublisherQos pubqos = efd::PUBLISHER_QOS_DEFAULT;
-    _info->_publisher = _info->_participant->create_publisher(pubqos, nullptr);
+    _info->_publisher = participant->create_publisher(pubqos, nullptr);
     if (_info->_publisher == nullptr) {
       std::cerr << "Failed to create Publisher" << std::endl;
       return false;
@@ -159,7 +143,7 @@ namespace ros2 {
       topic_name += _parent + "/";
     topic_name += _name;
     topic_name += publisher_type;
-    _info->_topic = _info->_participant->create_topic(topic_name, _info->_type->getName(), tqos);
+    _info->_topic = participant->create_topic(topic_name, _info->_type->getName(), tqos);
     if (_info->_topic == nullptr) {
         std::cerr << "Failed to create Topic" << std::endl;
         return false;
@@ -182,18 +166,11 @@ namespace ros2 {
         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);
+    auto participant = ROS2::GetDdsDomainParticipant()->GetDomainParticipant();
+    _point_cloud->_type.register_type(participant);
 
     efd::PublisherQos pubqos = efd::PUBLISHER_QOS_DEFAULT;
-    _point_cloud->_publisher = _point_cloud->_participant->create_publisher(pubqos, nullptr);
+    _point_cloud->_publisher = participant->create_publisher(pubqos, nullptr);
     if (_point_cloud->_publisher == nullptr) {
       std::cerr << "Failed to create Publisher" << std::endl;
       return false;
@@ -207,7 +184,7 @@ namespace ros2 {
       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);
+    _point_cloud->_topic = 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;
@@ -519,17 +496,16 @@ namespace ros2 {
       if (!_impl)
           return;
 
+      auto participant = ROS2::GetDdsDomainParticipant()->GetDomainParticipant();
       if (_impl->_datawriter)
           _impl->_publisher->delete_datawriter(_impl->_datawriter);
 
       if (_impl->_publisher)
-          _impl->_participant->delete_publisher(_impl->_publisher);
+          participant->delete_publisher(_impl->_publisher);
 
       if (_impl->_topic)
-          _impl->_participant->delete_topic(_impl->_topic);
+          participant->delete_topic(_impl->_topic);
 
-      if (_impl->_participant)
-          efd::DomainParticipantFactory::get_instance()->delete_participant(_impl->_participant);
 
       if (!_info)
           return;
@@ -538,13 +514,10 @@ namespace ros2 {
           _info->_publisher->delete_datawriter(_info->_datawriter);
 
       if (_info->_publisher)
-          _info->_participant->delete_publisher(_info->_publisher);
+          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);
+          participant->delete_topic(_info->_topic);
 
       if (!_point_cloud)
           return;
@@ -553,13 +526,10 @@ namespace ros2 {
           _point_cloud->_publisher->delete_datawriter(_point_cloud->_datawriter);
 
       if (_point_cloud->_publisher)
-          _point_cloud->_participant->delete_publisher(_point_cloud->_publisher);
+          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);
+          participant->delete_topic(_point_cloud->_topic);
   }
 
   CarlaDVSCameraPublisher::CarlaDVSCameraPublisher(const CarlaDVSCameraPublisher& other) {
diff --git a/LibCarla/source/carla/ros2/publishers/CarlaDepthCameraPublisher.cpp b/LibCarla/source/carla/ros2/publishers/CarlaDepthCameraPublisher.cpp
index 895fab63c..e742c0f3e 100644
--- a/LibCarla/source/carla/ros2/publishers/CarlaDepthCameraPublisher.cpp
+++ b/LibCarla/source/carla/ros2/publishers/CarlaDepthCameraPublisher.cpp
@@ -14,8 +14,8 @@
 #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 <carla/ros2/ROS2.h>
+#include <carla/ros2/impl/DdsDomainParticipantImpl.h>
 #include <fastdds/dds/publisher/qos/PublisherQos.hpp>
 #include <fastdds/dds/topic/qos/TopicQos.hpp>
 
@@ -32,7 +32,6 @@ namespace ros2 {
   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 };
@@ -42,7 +41,6 @@ namespace ros2 {
   };
 
   struct CarlaCameraInfoPublisherImpl {
-    efd::DomainParticipant* _participant { nullptr };
     efd::Publisher* _publisher { nullptr };
     efd::Topic* _topic { nullptr };
     efd::DataWriter* _datawriter { nullptr };
@@ -72,18 +70,12 @@ namespace ros2 {
         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);
+    auto participant = ROS2::GetDdsDomainParticipant()->GetDomainParticipant();
+
+    _impl->_type.register_type(participant);
 
     efd::PublisherQos pubqos = efd::PUBLISHER_QOS_DEFAULT;
-    _impl->_publisher = _impl->_participant->create_publisher(pubqos, nullptr);
+    _impl->_publisher = participant->create_publisher(pubqos, nullptr);
     if (_impl->_publisher == nullptr) {
       std::cerr << "Failed to create Publisher" << std::endl;
       return false;
@@ -97,7 +89,7 @@ namespace ros2 {
       topic_name += _parent + "/";
     topic_name += _name;
     topic_name += publisher_type;
-    _impl->_topic = _impl->_participant->create_topic(topic_name, _impl->_type->getName(), tqos);
+    _impl->_topic = participant->create_topic(topic_name, _impl->_type->getName(), tqos);
     if (_impl->_topic == nullptr) {
         std::cerr << "Failed to create Topic" << std::endl;
         return false;
@@ -121,18 +113,11 @@ namespace ros2 {
         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);
+      auto participant = ROS2::GetDdsDomainParticipant()->GetDomainParticipant();
+    _impl_info->_type.register_type(participant);
 
     efd::PublisherQos pubqos = efd::PUBLISHER_QOS_DEFAULT;
-    _impl_info->_publisher = _impl_info->_participant->create_publisher(pubqos, nullptr);
+    _impl_info->_publisher = participant->create_publisher(pubqos, nullptr);
     if (_impl_info->_publisher == nullptr) {
       std::cerr << "Failed to create Publisher" << std::endl;
       return false;
@@ -146,7 +131,7 @@ namespace ros2 {
       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);
+    _impl_info->_topic = 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;
@@ -348,17 +333,16 @@ namespace ros2 {
       if (!_impl)
           return;
 
+      auto participant = ROS2::GetDdsDomainParticipant()->GetDomainParticipant();
       if (_impl->_datawriter)
           _impl->_publisher->delete_datawriter(_impl->_datawriter);
 
       if (_impl->_publisher)
-          _impl->_participant->delete_publisher(_impl->_publisher);
+          participant->delete_publisher(_impl->_publisher);
 
       if (_impl->_topic)
-          _impl->_participant->delete_topic(_impl->_topic);
+          participant->delete_topic(_impl->_topic);
 
-      if (_impl->_participant)
-          efd::DomainParticipantFactory::get_instance()->delete_participant(_impl->_participant);
 
       if (!_impl_info)
         return;
@@ -367,13 +351,10 @@ namespace ros2 {
           _impl_info->_publisher->delete_datawriter(_impl_info->_datawriter);
 
       if (_impl_info->_publisher)
-          _impl_info->_participant->delete_publisher(_impl_info->_publisher);
+          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);
+          participant->delete_topic(_impl_info->_topic);
   }
 
   CarlaDepthCameraPublisher::CarlaDepthCameraPublisher(const CarlaDepthCameraPublisher& other) {
diff --git a/LibCarla/source/carla/ros2/publishers/CarlaGNSSPublisher.cpp b/LibCarla/source/carla/ros2/publishers/CarlaGNSSPublisher.cpp
index fc0762d11..55ab83984 100644
--- a/LibCarla/source/carla/ros2/publishers/CarlaGNSSPublisher.cpp
+++ b/LibCarla/source/carla/ros2/publishers/CarlaGNSSPublisher.cpp
@@ -13,8 +13,8 @@
 #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 <carla/ros2/ROS2.h>
+#include <carla/ros2/impl/DdsDomainParticipantImpl.h>
 #include <fastdds/dds/publisher/qos/PublisherQos.hpp>
 #include <fastdds/dds/topic/qos/TopicQos.hpp>
 
@@ -30,7 +30,6 @@ namespace ros2 {
   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 };
@@ -45,18 +44,12 @@ namespace ros2 {
         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);
+    auto participant = ROS2::GetDdsDomainParticipant()->GetDomainParticipant();
+
+    _impl->_type.register_type(participant);
 
     efd::PublisherQos pubqos = efd::PUBLISHER_QOS_DEFAULT;
-    _impl->_publisher = _impl->_participant->create_publisher(pubqos, nullptr);
+    _impl->_publisher = participant->create_publisher(pubqos, nullptr);
     if (_impl->_publisher == nullptr) {
       std::cerr << "Failed to create Publisher" << std::endl;
       return false;
@@ -68,7 +61,7 @@ namespace ros2 {
     if (!_parent.empty())
       topic_name += _parent + "/";
     topic_name += _name;
-    _impl->_topic = _impl->_participant->create_topic(topic_name, _impl->_type->getName(), tqos);
+    _impl->_topic = participant->create_topic(topic_name, _impl->_type->getName(), tqos);
     if (_impl->_topic == nullptr) {
         std::cerr << "Failed to create Topic" << std::endl;
         return false;
@@ -173,17 +166,16 @@ namespace ros2 {
       if (!_impl)
           return;
 
+      auto participant = ROS2::GetDdsDomainParticipant()->GetDomainParticipant();
       if (_impl->_datawriter)
           _impl->_publisher->delete_datawriter(_impl->_datawriter);
 
       if (_impl->_publisher)
-          _impl->_participant->delete_publisher(_impl->_publisher);
+          participant->delete_publisher(_impl->_publisher);
 
       if (_impl->_topic)
-          _impl->_participant->delete_topic(_impl->_topic);
+          participant->delete_topic(_impl->_topic);
 
-      if (_impl->_participant)
-          efd::DomainParticipantFactory::get_instance()->delete_participant(_impl->_participant);
   }
 
   CarlaGNSSPublisher::CarlaGNSSPublisher(const CarlaGNSSPublisher& other) {
diff --git a/LibCarla/source/carla/ros2/publishers/CarlaIMUPublisher.cpp b/LibCarla/source/carla/ros2/publishers/CarlaIMUPublisher.cpp
index b7c305eb3..59217b467 100644
--- a/LibCarla/source/carla/ros2/publishers/CarlaIMUPublisher.cpp
+++ b/LibCarla/source/carla/ros2/publishers/CarlaIMUPublisher.cpp
@@ -13,8 +13,8 @@
 #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 <carla/ros2/ROS2.h>
+#include <carla/ros2/impl/DdsDomainParticipantImpl.h>
 #include <fastdds/dds/publisher/qos/PublisherQos.hpp>
 #include <fastdds/dds/topic/qos/TopicQos.hpp>
 
@@ -30,7 +30,6 @@ namespace ros2 {
   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 };
@@ -45,18 +44,12 @@ namespace ros2 {
         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);
+    auto participant = ROS2::GetDdsDomainParticipant()->GetDomainParticipant();
+
+    _impl->_type.register_type(participant);
 
     efd::PublisherQos pubqos = efd::PUBLISHER_QOS_DEFAULT;
-    _impl->_publisher = _impl->_participant->create_publisher(pubqos, nullptr);
+    _impl->_publisher = participant->create_publisher(pubqos, nullptr);
     if (_impl->_publisher == nullptr) {
       std::cerr << "Failed to create Publisher" << std::endl;
       return false;
@@ -68,7 +61,7 @@ namespace ros2 {
     if (!_parent.empty())
       topic_name += _parent + "/";
     topic_name += _name;
-    _impl->_topic = _impl->_participant->create_topic(topic_name, _impl->_type->getName(), tqos);
+    _impl->_topic = participant->create_topic(topic_name, _impl->_type->getName(), tqos);
     if (_impl->_topic == nullptr) {
         std::cerr << "Failed to create Topic" << std::endl;
         return false;
@@ -175,7 +168,7 @@ namespace ros2 {
     geometry_msgs::msg::Quaternion orientation;
 
     const float rx = 0.0f;                           // pitch
-    const float ry = (M_PIf32 / 2.0f) - compass;     // yaw
+    const float ry = (float(M_PI_2) / 2.0f) - compass;     // yaw
     const float rz = 0.0f;                           // roll
 
     const float cr = cosf(rz * 0.5f);
@@ -206,17 +199,16 @@ namespace ros2 {
       if (!_impl)
           return;
 
+      auto participant = ROS2::GetDdsDomainParticipant()->GetDomainParticipant();
       if (_impl->_datawriter)
           _impl->_publisher->delete_datawriter(_impl->_datawriter);
 
       if (_impl->_publisher)
-          _impl->_participant->delete_publisher(_impl->_publisher);
+          participant->delete_publisher(_impl->_publisher);
 
       if (_impl->_topic)
-          _impl->_participant->delete_topic(_impl->_topic);
+          participant->delete_topic(_impl->_topic);
 
-      if (_impl->_participant)
-          efd::DomainParticipantFactory::get_instance()->delete_participant(_impl->_participant);
   }
 
   CarlaIMUPublisher::CarlaIMUPublisher(const CarlaIMUPublisher& other) {
diff --git a/LibCarla/source/carla/ros2/publishers/CarlaISCameraPublisher.cpp b/LibCarla/source/carla/ros2/publishers/CarlaISCameraPublisher.cpp
index 6c582a302..d5087e118 100644
--- a/LibCarla/source/carla/ros2/publishers/CarlaISCameraPublisher.cpp
+++ b/LibCarla/source/carla/ros2/publishers/CarlaISCameraPublisher.cpp
@@ -14,8 +14,8 @@
 #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 <carla/ros2/ROS2.h>
+#include <carla/ros2/impl/DdsDomainParticipantImpl.h>
 #include <fastdds/dds/publisher/qos/PublisherQos.hpp>
 #include <fastdds/dds/topic/qos/TopicQos.hpp>
 
@@ -31,7 +31,6 @@ namespace ros2 {
   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 };
@@ -41,7 +40,6 @@ namespace ros2 {
   };
 
   struct CarlaCameraInfoPublisherImpl {
-    efd::DomainParticipant* _participant { nullptr };
     efd::Publisher* _publisher { nullptr };
     efd::Topic* _topic { nullptr };
     efd::DataWriter* _datawriter { nullptr };
@@ -70,18 +68,12 @@ namespace ros2 {
         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);
+    auto participant = ROS2::GetDdsDomainParticipant()->GetDomainParticipant();
+
+    _impl->_type.register_type(participant);
 
     efd::PublisherQos pubqos = efd::PUBLISHER_QOS_DEFAULT;
-    _impl->_publisher = _impl->_participant->create_publisher(pubqos, nullptr);
+    _impl->_publisher = participant->create_publisher(pubqos, nullptr);
     if (_impl->_publisher == nullptr) {
       std::cerr << "Failed to create Publisher" << std::endl;
       return false;
@@ -95,7 +87,7 @@ namespace ros2 {
       topic_name += _parent + "/";
     topic_name += _name;
     topic_name += publisher_type;
-    _impl->_topic = _impl->_participant->create_topic(topic_name, _impl->_type->getName(), tqos);
+    _impl->_topic = participant->create_topic(topic_name, _impl->_type->getName(), tqos);
     if (_impl->_topic == nullptr) {
         std::cerr << "Failed to create Topic" << std::endl;
         return false;
@@ -119,18 +111,11 @@ namespace ros2 {
         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);
+      auto participant = ROS2::GetDdsDomainParticipant()->GetDomainParticipant();
+    _impl_info->_type.register_type(participant);
 
     efd::PublisherQos pubqos = efd::PUBLISHER_QOS_DEFAULT;
-    _impl_info->_publisher = _impl_info->_participant->create_publisher(pubqos, nullptr);
+    _impl_info->_publisher = participant->create_publisher(pubqos, nullptr);
     if (_impl_info->_publisher == nullptr) {
       std::cerr << "Failed to create Publisher" << std::endl;
       return false;
@@ -144,7 +129,7 @@ namespace ros2 {
       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);
+    _impl_info->_topic = 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;
@@ -347,17 +332,16 @@ namespace ros2 {
       if (!_impl)
           return;
 
+      auto participant = ROS2::GetDdsDomainParticipant()->GetDomainParticipant();
       if (_impl->_datawriter)
           _impl->_publisher->delete_datawriter(_impl->_datawriter);
 
       if (_impl->_publisher)
-          _impl->_participant->delete_publisher(_impl->_publisher);
+          participant->delete_publisher(_impl->_publisher);
 
       if (_impl->_topic)
-          _impl->_participant->delete_topic(_impl->_topic);
+          participant->delete_topic(_impl->_topic);
 
-      if (_impl->_participant)
-          efd::DomainParticipantFactory::get_instance()->delete_participant(_impl->_participant);
 
       if (!_impl_info)
         return;
@@ -366,13 +350,10 @@ namespace ros2 {
           _impl_info->_publisher->delete_datawriter(_impl_info->_datawriter);
 
       if (_impl_info->_publisher)
-          _impl_info->_participant->delete_publisher(_impl_info->_publisher);
+          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);
+          participant->delete_topic(_impl_info->_topic);
   }
 
   CarlaISCameraPublisher::CarlaISCameraPublisher(const CarlaISCameraPublisher& other) {
diff --git a/LibCarla/source/carla/ros2/publishers/CarlaLidarPublisher.cpp b/LibCarla/source/carla/ros2/publishers/CarlaLidarPublisher.cpp
index 4a8c5d53a..2f9c06358 100644
--- a/LibCarla/source/carla/ros2/publishers/CarlaLidarPublisher.cpp
+++ b/LibCarla/source/carla/ros2/publishers/CarlaLidarPublisher.cpp
@@ -13,8 +13,8 @@
 #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 <carla/ros2/ROS2.h>
+#include <carla/ros2/impl/DdsDomainParticipantImpl.h>
 #include <fastdds/dds/publisher/qos/PublisherQos.hpp>
 #include <fastdds/dds/topic/qos/TopicQos.hpp>
 
@@ -30,7 +30,6 @@ namespace ros2 {
   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 };
@@ -45,18 +44,12 @@ namespace ros2 {
         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);
+    auto participant = ROS2::GetDdsDomainParticipant()->GetDomainParticipant();
+
+    _impl->_type.register_type(participant);
 
     efd::PublisherQos pubqos = efd::PUBLISHER_QOS_DEFAULT;
-    _impl->_publisher = _impl->_participant->create_publisher(pubqos, nullptr);
+    _impl->_publisher = participant->create_publisher(pubqos, nullptr);
     if (_impl->_publisher == nullptr) {
       std::cerr << "Failed to create Publisher" << std::endl;
       return false;
@@ -68,7 +61,7 @@ namespace ros2 {
     if (!_parent.empty())
       topic_name += _parent + "/";
     topic_name += _name;
-    _impl->_topic = _impl->_participant->create_topic(topic_name, _impl->_type->getName(), tqos);
+    _impl->_topic = participant->create_topic(topic_name, _impl->_type->getName(), tqos);
     if (_impl->_topic == nullptr) {
         std::cerr << "Failed to create Topic" << std::endl;
         return false;
@@ -214,17 +207,16 @@ void CarlaLidarPublisher::SetData(int32_t seconds, uint32_t nanoseconds, size_t
       if (!_impl)
           return;
 
+      auto participant = ROS2::GetDdsDomainParticipant()->GetDomainParticipant();
       if (_impl->_datawriter)
           _impl->_publisher->delete_datawriter(_impl->_datawriter);
 
       if (_impl->_publisher)
-          _impl->_participant->delete_publisher(_impl->_publisher);
+          participant->delete_publisher(_impl->_publisher);
 
       if (_impl->_topic)
-          _impl->_participant->delete_topic(_impl->_topic);
+          participant->delete_topic(_impl->_topic);
 
-      if (_impl->_participant)
-          efd::DomainParticipantFactory::get_instance()->delete_participant(_impl->_participant);
   }
 
   CarlaLidarPublisher::CarlaLidarPublisher(const CarlaLidarPublisher& other) {
diff --git a/LibCarla/source/carla/ros2/publishers/CarlaLineInvasionPublisher.cpp b/LibCarla/source/carla/ros2/publishers/CarlaLineInvasionPublisher.cpp
index ce906048c..2f754b175 100644
--- a/LibCarla/source/carla/ros2/publishers/CarlaLineInvasionPublisher.cpp
+++ b/LibCarla/source/carla/ros2/publishers/CarlaLineInvasionPublisher.cpp
@@ -13,8 +13,8 @@
 #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 <carla/ros2/ROS2.h>
+#include <carla/ros2/impl/DdsDomainParticipantImpl.h>
 #include <fastdds/dds/publisher/qos/PublisherQos.hpp>
 #include <fastdds/dds/topic/qos/TopicQos.hpp>
 
@@ -30,7 +30,6 @@ namespace ros2 {
   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 };
@@ -45,18 +44,12 @@ namespace ros2 {
         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);
+    auto participant = ROS2::GetDdsDomainParticipant()->GetDomainParticipant();
+
+    _impl->_type.register_type(participant);
 
     efd::PublisherQos pubqos = efd::PUBLISHER_QOS_DEFAULT;
-    _impl->_publisher = _impl->_participant->create_publisher(pubqos, nullptr);
+    _impl->_publisher = participant->create_publisher(pubqos, nullptr);
     if (_impl->_publisher == nullptr) {
       std::cerr << "Failed to create Publisher" << std::endl;
       return false;
@@ -68,7 +61,7 @@ namespace ros2 {
     if (!_parent.empty())
       topic_name += _parent + "/";
     topic_name += _name;
-    _impl->_topic = _impl->_participant->create_topic(topic_name, _impl->_type->getName(), tqos);
+    _impl->_topic = participant->create_topic(topic_name, _impl->_type->getName(), tqos);
     if (_impl->_topic == nullptr) {
         std::cerr << "Failed to create Topic" << std::endl;
         return false;
@@ -171,17 +164,16 @@ namespace ros2 {
       if (!_impl)
           return;
 
+      auto participant = ROS2::GetDdsDomainParticipant()->GetDomainParticipant();
       if (_impl->_datawriter)
           _impl->_publisher->delete_datawriter(_impl->_datawriter);
 
       if (_impl->_publisher)
-          _impl->_participant->delete_publisher(_impl->_publisher);
+          participant->delete_publisher(_impl->_publisher);
 
       if (_impl->_topic)
-          _impl->_participant->delete_topic(_impl->_topic);
+          participant->delete_topic(_impl->_topic);
 
-      if (_impl->_participant)
-          efd::DomainParticipantFactory::get_instance()->delete_participant(_impl->_participant);
   }
 
   CarlaLineInvasionPublisher::CarlaLineInvasionPublisher(const CarlaLineInvasionPublisher& other) {
diff --git a/LibCarla/source/carla/ros2/publishers/CarlaMapSensorPublisher.cpp b/LibCarla/source/carla/ros2/publishers/CarlaMapSensorPublisher.cpp
index 536576f94..fcd2f2508 100644
--- a/LibCarla/source/carla/ros2/publishers/CarlaMapSensorPublisher.cpp
+++ b/LibCarla/source/carla/ros2/publishers/CarlaMapSensorPublisher.cpp
@@ -13,8 +13,8 @@
 #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 <carla/ros2/ROS2.h>
+#include <carla/ros2/impl/DdsDomainParticipantImpl.h>
 #include <fastdds/dds/publisher/qos/PublisherQos.hpp>
 #include <fastdds/dds/topic/qos/TopicQos.hpp>
 
@@ -30,7 +30,6 @@ namespace ros2 {
   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 };
@@ -45,18 +44,12 @@ namespace ros2 {
         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);
+    auto participant = ROS2::GetDdsDomainParticipant()->GetDomainParticipant();
+
+    _impl->_type.register_type(participant);
 
     efd::PublisherQos pubqos = efd::PUBLISHER_QOS_DEFAULT;
-    _impl->_publisher = _impl->_participant->create_publisher(pubqos, nullptr);
+    _impl->_publisher = participant->create_publisher(pubqos, nullptr);
     if (_impl->_publisher == nullptr) {
       std::cerr << "Failed to create Publisher" << std::endl;
       return false;
@@ -68,7 +61,7 @@ namespace ros2 {
     if (!_parent.empty())
       topic_name += _parent + "/";
     topic_name += _name;
-    _impl->_topic = _impl->_participant->create_topic(topic_name, _impl->_type->getName(), tqos);
+    _impl->_topic = participant->create_topic(topic_name, _impl->_type->getName(), tqos);
     if (_impl->_topic == nullptr) {
         std::cerr << "Failed to create Topic" << std::endl;
         return false;
@@ -162,17 +155,16 @@ namespace ros2 {
       if (!_impl)
           return;
 
+      auto participant = ROS2::GetDdsDomainParticipant()->GetDomainParticipant();
       if (_impl->_datawriter)
           _impl->_publisher->delete_datawriter(_impl->_datawriter);
 
       if (_impl->_publisher)
-          _impl->_participant->delete_publisher(_impl->_publisher);
+          participant->delete_publisher(_impl->_publisher);
 
       if (_impl->_topic)
-          _impl->_participant->delete_topic(_impl->_topic);
+          participant->delete_topic(_impl->_topic);
 
-      if (_impl->_participant)
-          efd::DomainParticipantFactory::get_instance()->delete_participant(_impl->_participant);
   }
 
   CarlaMapSensorPublisher::CarlaMapSensorPublisher(const CarlaMapSensorPublisher& other) {
diff --git a/LibCarla/source/carla/ros2/publishers/CarlaNormalsCameraPublisher.cpp b/LibCarla/source/carla/ros2/publishers/CarlaNormalsCameraPublisher.cpp
index 94dabce72..74b7f9912 100644
--- a/LibCarla/source/carla/ros2/publishers/CarlaNormalsCameraPublisher.cpp
+++ b/LibCarla/source/carla/ros2/publishers/CarlaNormalsCameraPublisher.cpp
@@ -14,8 +14,8 @@
 #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 <carla/ros2/ROS2.h>
+#include <carla/ros2/impl/DdsDomainParticipantImpl.h>
 #include <fastdds/dds/publisher/qos/PublisherQos.hpp>
 #include <fastdds/dds/topic/qos/TopicQos.hpp>
 
@@ -32,7 +32,6 @@ namespace ros2 {
   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 };
@@ -42,7 +41,6 @@ namespace ros2 {
   };
 
   struct CarlaCameraInfoPublisherImpl {
-    efd::DomainParticipant* _participant { nullptr };
     efd::Publisher* _publisher { nullptr };
     efd::Topic* _topic { nullptr };
     efd::DataWriter* _datawriter { nullptr };
@@ -72,18 +70,12 @@ namespace ros2 {
         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);
+    auto participant = ROS2::GetDdsDomainParticipant()->GetDomainParticipant();
+
+    _impl->_type.register_type(participant);
 
     efd::PublisherQos pubqos = efd::PUBLISHER_QOS_DEFAULT;
-    _impl->_publisher = _impl->_participant->create_publisher(pubqos, nullptr);
+    _impl->_publisher = participant->create_publisher(pubqos, nullptr);
     if (_impl->_publisher == nullptr) {
       std::cerr << "Failed to create Publisher" << std::endl;
       return false;
@@ -97,7 +89,7 @@ namespace ros2 {
       topic_name += _parent + "/";
     topic_name += _name;
     topic_name += publisher_type;
-    _impl->_topic = _impl->_participant->create_topic(topic_name, _impl->_type->getName(), tqos);
+    _impl->_topic = participant->create_topic(topic_name, _impl->_type->getName(), tqos);
     if (_impl->_topic == nullptr) {
         std::cerr << "Failed to create Topic" << std::endl;
         return false;
@@ -121,18 +113,11 @@ namespace ros2 {
         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);
+      auto participant = ROS2::GetDdsDomainParticipant()->GetDomainParticipant();
+    _impl_info->_type.register_type(participant);
 
     efd::PublisherQos pubqos = efd::PUBLISHER_QOS_DEFAULT;
-    _impl_info->_publisher = _impl_info->_participant->create_publisher(pubqos, nullptr);
+    _impl_info->_publisher = participant->create_publisher(pubqos, nullptr);
     if (_impl_info->_publisher == nullptr) {
       std::cerr << "Failed to create Publisher" << std::endl;
       return false;
@@ -146,7 +131,7 @@ namespace ros2 {
       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);
+    _impl_info->_topic = 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;
@@ -348,17 +333,16 @@ namespace ros2 {
       if (!_impl)
           return;
 
+      auto participant = ROS2::GetDdsDomainParticipant()->GetDomainParticipant();
       if (_impl->_datawriter)
           _impl->_publisher->delete_datawriter(_impl->_datawriter);
 
       if (_impl->_publisher)
-          _impl->_participant->delete_publisher(_impl->_publisher);
+          participant->delete_publisher(_impl->_publisher);
 
       if (_impl->_topic)
-          _impl->_participant->delete_topic(_impl->_topic);
+          participant->delete_topic(_impl->_topic);
 
-      if (_impl->_participant)
-          efd::DomainParticipantFactory::get_instance()->delete_participant(_impl->_participant);
 
       if (!_impl_info)
         return;
@@ -367,13 +351,10 @@ namespace ros2 {
           _impl_info->_publisher->delete_datawriter(_impl_info->_datawriter);
 
       if (_impl_info->_publisher)
-          _impl_info->_participant->delete_publisher(_impl_info->_publisher);
+          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);
+          participant->delete_topic(_impl_info->_topic);
   }
 
   CarlaNormalsCameraPublisher::CarlaNormalsCameraPublisher(const CarlaNormalsCameraPublisher& other) {
diff --git a/LibCarla/source/carla/ros2/publishers/CarlaOpticalFlowCameraPublisher.cpp b/LibCarla/source/carla/ros2/publishers/CarlaOpticalFlowCameraPublisher.cpp
index afa2e125c..c6b93de75 100644
--- a/LibCarla/source/carla/ros2/publishers/CarlaOpticalFlowCameraPublisher.cpp
+++ b/LibCarla/source/carla/ros2/publishers/CarlaOpticalFlowCameraPublisher.cpp
@@ -15,8 +15,8 @@
 #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 <carla/ros2/ROS2.h>
+#include <carla/ros2/impl/DdsDomainParticipantImpl.h>
 #include <fastdds/dds/publisher/qos/PublisherQos.hpp>
 #include <fastdds/dds/topic/qos/TopicQos.hpp>
 
@@ -36,7 +36,6 @@ 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 };
@@ -46,7 +45,6 @@ namespace ros2 {
   };
 
   struct CarlaCameraInfoPublisherImpl {
-    efd::DomainParticipant* _participant { nullptr };
     efd::Publisher* _publisher { nullptr };
     efd::Topic* _topic { nullptr };
     efd::DataWriter* _datawriter { nullptr };
@@ -76,18 +74,12 @@ namespace ros2 {
         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);
+    auto participant = ROS2::GetDdsDomainParticipant()->GetDomainParticipant();
+
+    _impl->_type.register_type(participant);
 
     efd::PublisherQos pubqos = efd::PUBLISHER_QOS_DEFAULT;
-    _impl->_publisher = _impl->_participant->create_publisher(pubqos, nullptr);
+    _impl->_publisher = participant->create_publisher(pubqos, nullptr);
     if (_impl->_publisher == nullptr) {
       std::cerr << "Failed to create Publisher" << std::endl;
       return false;
@@ -101,7 +93,7 @@ namespace ros2 {
       topic_name += _parent + "/";
     topic_name += _name;
     topic_name += publisher_type;
-    _impl->_topic = _impl->_participant->create_topic(topic_name, _impl->_type->getName(), tqos);
+    _impl->_topic = participant->create_topic(topic_name, _impl->_type->getName(), tqos);
     if (_impl->_topic == nullptr) {
         std::cerr << "Failed to create Topic" << std::endl;
         return false;
@@ -125,18 +117,11 @@ namespace ros2 {
         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);
+      auto participant = ROS2::GetDdsDomainParticipant()->GetDomainParticipant();
+    _impl_info->_type.register_type(participant);
 
     efd::PublisherQos pubqos = efd::PUBLISHER_QOS_DEFAULT;
-    _impl_info->_publisher = _impl_info->_participant->create_publisher(pubqos, nullptr);
+    _impl_info->_publisher = participant->create_publisher(pubqos, nullptr);
     if (_impl_info->_publisher == nullptr) {
       std::cerr << "Failed to create Publisher" << std::endl;
       return false;
@@ -150,7 +135,7 @@ namespace ros2 {
       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);
+    _impl_info->_topic = 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;
@@ -430,17 +415,16 @@ namespace ros2 {
       if (!_impl)
           return;
 
+      auto participant = ROS2::GetDdsDomainParticipant()->GetDomainParticipant();
       if (_impl->_datawriter)
           _impl->_publisher->delete_datawriter(_impl->_datawriter);
 
       if (_impl->_publisher)
-          _impl->_participant->delete_publisher(_impl->_publisher);
+          participant->delete_publisher(_impl->_publisher);
 
       if (_impl->_topic)
-          _impl->_participant->delete_topic(_impl->_topic);
+          participant->delete_topic(_impl->_topic);
 
-      if (_impl->_participant)
-          efd::DomainParticipantFactory::get_instance()->delete_participant(_impl->_participant);
 
       if (!_impl_info)
         return;
@@ -449,13 +433,10 @@ namespace ros2 {
           _impl_info->_publisher->delete_datawriter(_impl_info->_datawriter);
 
       if (_impl_info->_publisher)
-          _impl_info->_participant->delete_publisher(_impl_info->_publisher);
+          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);
+          participant->delete_topic(_impl_info->_topic);
   }
 
   CarlaOpticalFlowCameraPublisher::CarlaOpticalFlowCameraPublisher(const CarlaOpticalFlowCameraPublisher& other) {
diff --git a/LibCarla/source/carla/ros2/publishers/CarlaRGBCameraPublisher.cpp b/LibCarla/source/carla/ros2/publishers/CarlaRGBCameraPublisher.cpp
index 36a6d7ad1..e7e3f4039 100644
--- a/LibCarla/source/carla/ros2/publishers/CarlaRGBCameraPublisher.cpp
+++ b/LibCarla/source/carla/ros2/publishers/CarlaRGBCameraPublisher.cpp
@@ -14,8 +14,8 @@
 #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 <carla/ros2/ROS2.h>
+#include <carla/ros2/impl/DdsDomainParticipantImpl.h>
 #include <fastdds/dds/publisher/qos/PublisherQos.hpp>
 #include <fastdds/dds/topic/qos/TopicQos.hpp>
 
@@ -32,7 +32,6 @@ namespace ros2 {
   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 };
@@ -42,7 +41,6 @@ namespace ros2 {
   };
 
   struct CarlaCameraInfoPublisherImpl {
-    efd::DomainParticipant* _participant { nullptr };
     efd::Publisher* _publisher { nullptr };
     efd::Topic* _topic { nullptr };
     efd::DataWriter* _datawriter { nullptr };
@@ -72,18 +70,12 @@ namespace ros2 {
         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);
+    auto participant = ROS2::GetDdsDomainParticipant()->GetDomainParticipant();
+
+    _impl->_type.register_type(participant);
 
     efd::PublisherQos pubqos = efd::PUBLISHER_QOS_DEFAULT;
-    _impl->_publisher = _impl->_participant->create_publisher(pubqos, nullptr);
+    _impl->_publisher = participant->create_publisher(pubqos, nullptr);
     if (_impl->_publisher == nullptr) {
       std::cerr << "Failed to create Publisher" << std::endl;
       return false;
@@ -97,7 +89,7 @@ namespace ros2 {
       topic_name += _parent + "/";
     topic_name += _name;
     topic_name += publisher_type;
-    _impl->_topic = _impl->_participant->create_topic(topic_name, _impl->_type->getName(), tqos);
+    _impl->_topic = participant->create_topic(topic_name, _impl->_type->getName(), tqos);
     if (_impl->_topic == nullptr) {
         std::cerr << "Failed to create Topic" << std::endl;
         return false;
@@ -121,18 +113,11 @@ namespace ros2 {
         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);
+      auto participant = ROS2::GetDdsDomainParticipant()->GetDomainParticipant();
+    _impl_info->_type.register_type(participant);
 
     efd::PublisherQos pubqos = efd::PUBLISHER_QOS_DEFAULT;
-    _impl_info->_publisher = _impl_info->_participant->create_publisher(pubqos, nullptr);
+    _impl_info->_publisher = participant->create_publisher(pubqos, nullptr);
     if (_impl_info->_publisher == nullptr) {
       std::cerr << "Failed to create Publisher" << std::endl;
       return false;
@@ -146,7 +131,7 @@ namespace ros2 {
       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);
+    _impl_info->_topic = 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;
@@ -350,17 +335,16 @@ void CarlaRGBCameraPublisher::SetImageData(int32_t seconds, uint32_t nanoseconds
       if (!_impl)
           return;
 
+      auto participant = ROS2::GetDdsDomainParticipant()->GetDomainParticipant();
       if (_impl->_datawriter)
           _impl->_publisher->delete_datawriter(_impl->_datawriter);
 
       if (_impl->_publisher)
-          _impl->_participant->delete_publisher(_impl->_publisher);
+          participant->delete_publisher(_impl->_publisher);
 
       if (_impl->_topic)
-          _impl->_participant->delete_topic(_impl->_topic);
+          participant->delete_topic(_impl->_topic);
 
-      if (_impl->_participant)
-          efd::DomainParticipantFactory::get_instance()->delete_participant(_impl->_participant);
 
       if (!_impl_info)
         return;
@@ -369,13 +353,10 @@ void CarlaRGBCameraPublisher::SetImageData(int32_t seconds, uint32_t nanoseconds
           _impl_info->_publisher->delete_datawriter(_impl_info->_datawriter);
 
       if (_impl_info->_publisher)
-          _impl_info->_participant->delete_publisher(_impl_info->_publisher);
+          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);
+          participant->delete_topic(_impl_info->_topic);
   }
 
   CarlaRGBCameraPublisher::CarlaRGBCameraPublisher(const CarlaRGBCameraPublisher& other) {
diff --git a/LibCarla/source/carla/ros2/publishers/CarlaRadarPublisher.cpp b/LibCarla/source/carla/ros2/publishers/CarlaRadarPublisher.cpp
index 5ef6fd3e9..5b6704b0c 100644
--- a/LibCarla/source/carla/ros2/publishers/CarlaRadarPublisher.cpp
+++ b/LibCarla/source/carla/ros2/publishers/CarlaRadarPublisher.cpp
@@ -14,8 +14,8 @@
 #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 <carla/ros2/ROS2.h>
+#include <carla/ros2/impl/DdsDomainParticipantImpl.h>
 #include <fastdds/dds/publisher/qos/PublisherQos.hpp>
 #include <fastdds/dds/topic/qos/TopicQos.hpp>
 
@@ -32,7 +32,6 @@ namespace ros2 {
   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 };
@@ -54,18 +53,12 @@ namespace ros2 {
         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);
+    auto participant = ROS2::GetDdsDomainParticipant()->GetDomainParticipant();
+
+    _impl->_type.register_type(participant);
 
     efd::PublisherQos pubqos = efd::PUBLISHER_QOS_DEFAULT;
-    _impl->_publisher = _impl->_participant->create_publisher(pubqos, nullptr);
+    _impl->_publisher = participant->create_publisher(pubqos, nullptr);
     if (_impl->_publisher == nullptr) {
       std::cerr << "Failed to create Publisher" << std::endl;
       return false;
@@ -77,7 +70,7 @@ namespace ros2 {
     if (!_parent.empty())
       topic_name += _parent + "/";
     topic_name += _name;
-    _impl->_topic = _impl->_participant->create_topic(topic_name, _impl->_type->getName(), tqos);
+    _impl->_topic = participant->create_topic(topic_name, _impl->_type->getName(), tqos);
     if (_impl->_topic == nullptr) {
         std::cerr << "Failed to create Topic" << std::endl;
         return false;
@@ -241,17 +234,16 @@ void CarlaRadarPublisher::SetData(int32_t seconds, uint32_t nanoseconds, size_t
       if (!_impl)
           return;
 
+      auto participant = ROS2::GetDdsDomainParticipant()->GetDomainParticipant();
       if (_impl->_datawriter)
           _impl->_publisher->delete_datawriter(_impl->_datawriter);
 
       if (_impl->_publisher)
-          _impl->_participant->delete_publisher(_impl->_publisher);
+          participant->delete_publisher(_impl->_publisher);
 
       if (_impl->_topic)
-          _impl->_participant->delete_topic(_impl->_topic);
+          participant->delete_topic(_impl->_topic);
 
-      if (_impl->_participant)
-          efd::DomainParticipantFactory::get_instance()->delete_participant(_impl->_participant);
   }
 
   CarlaRadarPublisher::CarlaRadarPublisher(const CarlaRadarPublisher& other) {
diff --git a/LibCarla/source/carla/ros2/publishers/CarlaSSCameraPublisher.cpp b/LibCarla/source/carla/ros2/publishers/CarlaSSCameraPublisher.cpp
index b096a6b80..2b67d9a26 100644
--- a/LibCarla/source/carla/ros2/publishers/CarlaSSCameraPublisher.cpp
+++ b/LibCarla/source/carla/ros2/publishers/CarlaSSCameraPublisher.cpp
@@ -14,8 +14,8 @@
 #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 <carla/ros2/ROS2.h>
+#include <carla/ros2/impl/DdsDomainParticipantImpl.h>
 #include <fastdds/dds/publisher/qos/PublisherQos.hpp>
 #include <fastdds/dds/topic/qos/TopicQos.hpp>
 
@@ -31,7 +31,6 @@ namespace ros2 {
   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 };
@@ -41,7 +40,6 @@ namespace ros2 {
   };
 
   struct CarlaCameraInfoPublisherImpl {
-    efd::DomainParticipant* _participant { nullptr };
     efd::Publisher* _publisher { nullptr };
     efd::Topic* _topic { nullptr };
     efd::DataWriter* _datawriter { nullptr };
@@ -70,18 +68,12 @@ namespace ros2 {
         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);
+    auto participant = ROS2::GetDdsDomainParticipant()->GetDomainParticipant();
+
+    _impl->_type.register_type(participant);
 
     efd::PublisherQos pubqos = efd::PUBLISHER_QOS_DEFAULT;
-    _impl->_publisher = _impl->_participant->create_publisher(pubqos, nullptr);
+    _impl->_publisher = participant->create_publisher(pubqos, nullptr);
     if (_impl->_publisher == nullptr) {
       std::cerr << "Failed to create Publisher" << std::endl;
       return false;
@@ -95,7 +87,7 @@ namespace ros2 {
       topic_name += _parent + "/";
     topic_name += _name;
     topic_name += publisher_type;
-    _impl->_topic = _impl->_participant->create_topic(topic_name, _impl->_type->getName(), tqos);
+    _impl->_topic = participant->create_topic(topic_name, _impl->_type->getName(), tqos);
     if (_impl->_topic == nullptr) {
         std::cerr << "Failed to create Topic" << std::endl;
         return false;
@@ -119,18 +111,11 @@ namespace ros2 {
         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);
+      auto participant = ROS2::GetDdsDomainParticipant()->GetDomainParticipant();
+    _impl_info->_type.register_type(participant);
 
     efd::PublisherQos pubqos = efd::PUBLISHER_QOS_DEFAULT;
-    _impl_info->_publisher = _impl_info->_participant->create_publisher(pubqos, nullptr);
+    _impl_info->_publisher = participant->create_publisher(pubqos, nullptr);
     if (_impl_info->_publisher == nullptr) {
       std::cerr << "Failed to create Publisher" << std::endl;
       return false;
@@ -144,7 +129,7 @@ namespace ros2 {
       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);
+    _impl_info->_topic = 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;
@@ -347,17 +332,16 @@ namespace ros2 {
       if (!_impl)
           return;
 
+      auto participant = ROS2::GetDdsDomainParticipant()->GetDomainParticipant();
       if (_impl->_datawriter)
           _impl->_publisher->delete_datawriter(_impl->_datawriter);
 
       if (_impl->_publisher)
-          _impl->_participant->delete_publisher(_impl->_publisher);
+          participant->delete_publisher(_impl->_publisher);
 
       if (_impl->_topic)
-          _impl->_participant->delete_topic(_impl->_topic);
+          participant->delete_topic(_impl->_topic);
 
-      if (_impl->_participant)
-          efd::DomainParticipantFactory::get_instance()->delete_participant(_impl->_participant);
 
       if (!_impl_info)
         return;
@@ -366,13 +350,10 @@ namespace ros2 {
           _impl_info->_publisher->delete_datawriter(_impl_info->_datawriter);
 
       if (_impl_info->_publisher)
-          _impl_info->_participant->delete_publisher(_impl_info->_publisher);
+          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);
+          participant->delete_topic(_impl_info->_topic);
   }
 
   CarlaSSCameraPublisher::CarlaSSCameraPublisher(const CarlaSSCameraPublisher& other) {
diff --git a/LibCarla/source/carla/ros2/publishers/CarlaSemanticLidarPublisher.cpp b/LibCarla/source/carla/ros2/publishers/CarlaSemanticLidarPublisher.cpp
index 2a64776d0..65b6ee8ae 100644
--- a/LibCarla/source/carla/ros2/publishers/CarlaSemanticLidarPublisher.cpp
+++ b/LibCarla/source/carla/ros2/publishers/CarlaSemanticLidarPublisher.cpp
@@ -13,8 +13,8 @@
 #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 <carla/ros2/ROS2.h>
+#include <carla/ros2/impl/DdsDomainParticipantImpl.h>
 #include <fastdds/dds/publisher/qos/PublisherQos.hpp>
 #include <fastdds/dds/topic/qos/TopicQos.hpp>
 
@@ -31,7 +31,6 @@ namespace ros2 {
   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 };
@@ -45,18 +44,12 @@ namespace ros2 {
         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);
+    auto participant = ROS2::GetDdsDomainParticipant()->GetDomainParticipant();
+
+    _impl->_type.register_type(participant);
 
     efd::PublisherQos pubqos = efd::PUBLISHER_QOS_DEFAULT;
-    _impl->_publisher = _impl->_participant->create_publisher(pubqos, nullptr);
+    _impl->_publisher = participant->create_publisher(pubqos, nullptr);
     if (_impl->_publisher == nullptr) {
       std::cerr << "Failed to create Publisher" << std::endl;
       return false;
@@ -68,7 +61,7 @@ namespace ros2 {
     if (!_parent.empty())
       topic_name += _parent + "/";
     topic_name += _name;
-    _impl->_topic = _impl->_participant->create_topic(topic_name, _impl->_type->getName(), tqos);
+    _impl->_topic = participant->create_topic(topic_name, _impl->_type->getName(), tqos);
     if (_impl->_topic == nullptr) {
         std::cerr << "Failed to create Topic" << std::endl;
         return false;
@@ -224,17 +217,16 @@ void CarlaSemanticLidarPublisher::SetData(int32_t seconds, uint32_t nanoseconds,
       if (!_impl)
           return;
 
+      auto participant = ROS2::GetDdsDomainParticipant()->GetDomainParticipant();
       if (_impl->_datawriter)
           _impl->_publisher->delete_datawriter(_impl->_datawriter);
 
       if (_impl->_publisher)
-          _impl->_participant->delete_publisher(_impl->_publisher);
+          participant->delete_publisher(_impl->_publisher);
 
       if (_impl->_topic)
-          _impl->_participant->delete_topic(_impl->_topic);
+          participant->delete_topic(_impl->_topic);
 
-      if (_impl->_participant)
-          efd::DomainParticipantFactory::get_instance()->delete_participant(_impl->_participant);
   }
 
   CarlaSemanticLidarPublisher::CarlaSemanticLidarPublisher(const CarlaSemanticLidarPublisher& other) {
diff --git a/LibCarla/source/carla/ros2/publishers/CarlaSpeedometerSensor.cpp b/LibCarla/source/carla/ros2/publishers/CarlaSpeedometerSensor.cpp
index 042e1dcfc..0abf55601 100644
--- a/LibCarla/source/carla/ros2/publishers/CarlaSpeedometerSensor.cpp
+++ b/LibCarla/source/carla/ros2/publishers/CarlaSpeedometerSensor.cpp
@@ -13,8 +13,8 @@
 #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 <carla/ros2/ROS2.h>
+#include <carla/ros2/impl/DdsDomainParticipantImpl.h>
 #include <fastdds/dds/publisher/qos/PublisherQos.hpp>
 #include <fastdds/dds/topic/qos/TopicQos.hpp>
 
@@ -30,7 +30,6 @@ namespace ros2 {
   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 };
@@ -44,18 +43,12 @@ namespace ros2 {
         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);
+    auto participant = ROS2::GetDdsDomainParticipant()->GetDomainParticipant();
+
+    _impl->_type.register_type(participant);
 
     efd::PublisherQos pubqos = efd::PUBLISHER_QOS_DEFAULT;
-    _impl->_publisher = _impl->_participant->create_publisher(pubqos, nullptr);
+    _impl->_publisher = participant->create_publisher(pubqos, nullptr);
     if (_impl->_publisher == nullptr) {
       std::cerr << "Failed to create Publisher" << std::endl;
       return false;
@@ -67,7 +60,7 @@ namespace ros2 {
     if (!_parent.empty())
       topic_name += _parent + "/";
     topic_name += _name;
-    _impl->_topic = _impl->_participant->create_topic(topic_name, _impl->_type->getName(), tqos);
+    _impl->_topic = participant->create_topic(topic_name, _impl->_type->getName(), tqos);
     if (_impl->_topic == nullptr) {
         std::cerr << "Failed to create Topic" << std::endl;
         return false;
@@ -161,17 +154,16 @@ namespace ros2 {
       if (!_impl)
           return;
 
+      auto participant = ROS2::GetDdsDomainParticipant()->GetDomainParticipant();
       if (_impl->_datawriter)
           _impl->_publisher->delete_datawriter(_impl->_datawriter);
 
       if (_impl->_publisher)
-          _impl->_participant->delete_publisher(_impl->_publisher);
+          participant->delete_publisher(_impl->_publisher);
 
       if (_impl->_topic)
-          _impl->_participant->delete_topic(_impl->_topic);
+          participant->delete_topic(_impl->_topic);
 
-      if (_impl->_participant)
-          efd::DomainParticipantFactory::get_instance()->delete_participant(_impl->_participant);
   }
 
   CarlaSpeedometerSensor::CarlaSpeedometerSensor(const CarlaSpeedometerSensor& other) {
diff --git a/LibCarla/source/carla/ros2/publishers/CarlaTransformPublisher.cpp b/LibCarla/source/carla/ros2/publishers/CarlaTransformPublisher.cpp
index caf9d5339..3a3ed20bf 100644
--- a/LibCarla/source/carla/ros2/publishers/CarlaTransformPublisher.cpp
+++ b/LibCarla/source/carla/ros2/publishers/CarlaTransformPublisher.cpp
@@ -13,8 +13,8 @@
 #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 <carla/ros2/ROS2.h>
+#include <carla/ros2/impl/DdsDomainParticipantImpl.h>
 #include <fastdds/dds/publisher/qos/PublisherQos.hpp>
 #include <fastdds/dds/topic/qos/TopicQos.hpp>
 
@@ -30,7 +30,6 @@ namespace ros2 {
   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 };
@@ -50,18 +49,12 @@ namespace ros2 {
         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);
+    auto participant = ROS2::GetDdsDomainParticipant()->GetDomainParticipant();
+
+    _impl->_type.register_type(participant);
 
     efd::PublisherQos pubqos = efd::PUBLISHER_QOS_DEFAULT;
-    _impl->_publisher = _impl->_participant->create_publisher(pubqos, nullptr);
+    _impl->_publisher = participant->create_publisher(pubqos, nullptr);
     if (_impl->_publisher == nullptr) {
       std::cerr << "Failed to create Publisher" << std::endl;
       return false;
@@ -69,7 +62,7 @@ namespace ros2 {
 
     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);
+    _impl->_topic = participant->create_topic(topic_name, _impl->_type->getName(), tqos);
     if (_impl->_topic == nullptr) {
         std::cerr << "Failed to create Topic" << std::endl;
         return false;
@@ -161,9 +154,9 @@ namespace ros2 {
         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 rx = ((*rotation++) * -1.0f) * (float(M_PI_2) / 180.0f);
+        const float ry = ((*rotation++) * -1.0f) * (float(M_PI_2) / 180.0f);
+        const float rz = *rotation++ * (float(M_PI_2) / 180.0f);
 
         const float cr = cosf(rz * 0.5f);
         const float sr = sinf(rz * 0.5f);
@@ -211,17 +204,16 @@ namespace ros2 {
       if (!_impl)
           return;
 
+      auto participant = ROS2::GetDdsDomainParticipant()->GetDomainParticipant();
       if (_impl->_datawriter)
           _impl->_publisher->delete_datawriter(_impl->_datawriter);
 
       if (_impl->_publisher)
-          _impl->_participant->delete_publisher(_impl->_publisher);
+          participant->delete_publisher(_impl->_publisher);
 
       if (_impl->_topic)
-          _impl->_participant->delete_topic(_impl->_topic);
+          participant->delete_topic(_impl->_topic);
 
-      if (_impl->_participant)
-          efd::DomainParticipantFactory::get_instance()->delete_participant(_impl->_participant);
   }
 
   CarlaTransformPublisher::CarlaTransformPublisher(const CarlaTransformPublisher& other) {
diff --git a/LibCarla/source/carla/ros2/subscribers/CarlaEgoVehicleControlSubscriber.cpp b/LibCarla/source/carla/ros2/subscribers/CarlaEgoVehicleControlSubscriber.cpp
index 4a31b7e70..24af10756 100644
--- a/LibCarla/source/carla/ros2/subscribers/CarlaEgoVehicleControlSubscriber.cpp
+++ b/LibCarla/source/carla/ros2/subscribers/CarlaEgoVehicleControlSubscriber.cpp
@@ -13,8 +13,8 @@
 #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 <carla/ros2/ROS2.h>
+#include <carla/ros2/impl/DdsDomainParticipantImpl.h>
 #include <fastdds/dds/subscriber/qos/SubscriberQos.hpp>
 #include <fastdds/dds/topic/qos/TopicQos.hpp>
 
@@ -31,7 +31,6 @@ namespace ros2 {
   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 };
@@ -50,18 +49,12 @@ namespace ros2 {
         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);
+    auto participant = ROS2::GetDdsDomainParticipant()->GetDomainParticipant();
+
+    _impl->_type.register_type(participant);
 
     efd::SubscriberQos subqos = efd::SUBSCRIBER_QOS_DEFAULT;
-    _impl->_subscriber = _impl->_participant->create_subscriber(subqos, nullptr);
+    _impl->_subscriber = participant->create_subscriber(subqos, nullptr);
     if (_impl->_subscriber == nullptr) {
       std::cerr << "Failed to create Subscriber" << std::endl;
       return false;
@@ -75,7 +68,7 @@ namespace ros2 {
       topic_name += _parent + "/";
     topic_name += _name;
     topic_name += publisher_type;
-    _impl->_topic = _impl->_participant->create_topic(topic_name, _impl->_type->getName(), tqos);
+    _impl->_topic = participant->create_topic(topic_name, _impl->_type->getName(), tqos);
     if (_impl->_topic == nullptr) {
         std::cerr << "Failed to create Topic" << std::endl;
         return false;
@@ -191,17 +184,16 @@ namespace ros2 {
       if (!_impl)
           return;
 
+    auto participant = ROS2::GetDdsDomainParticipant()->GetDomainParticipant();
       if (_impl->_datareader)
           _impl->_subscriber->delete_datareader(_impl->_datareader);
 
       if (_impl->_subscriber)
-          _impl->_participant->delete_subscriber(_impl->_subscriber);
+          participant->delete_subscriber(_impl->_subscriber);
 
       if (_impl->_topic)
-          _impl->_participant->delete_topic(_impl->_topic);
+          participant->delete_topic(_impl->_topic);
 
-      if (_impl->_participant)
-          efd::DomainParticipantFactory::get_instance()->delete_participant(_impl->_participant);
   }
 
   CarlaEgoVehicleControlSubscriber::CarlaEgoVehicleControlSubscriber(const CarlaEgoVehicleControlSubscriber& other) {
diff --git a/LibCarla/source/carla/rss/RssCheck.cpp b/LibCarla/source/carla/rss/RssCheck.cpp
index 713cf9a62..fd8a781a7 100644
--- a/LibCarla/source/carla/rss/RssCheck.cpp
+++ b/LibCarla/source/carla/rss/RssCheck.cpp
@@ -57,14 +57,14 @@ EgoDynamicsOnRoute::EgoDynamicsOnRoute()
     time_since_epoch_check_end_ms(0.),
     ego_speed(0.),
     min_stopping_distance(0.),
-    ego_center({0., 0., 0.}),
+    ego_center(),
     ego_heading(0.),
     ego_heading_change(0.),
     ego_steering_angle(0.),
     ego_center_within_route(false),
     crossing_border(false),
     route_heading(0.),
-    route_nominal_center({0., 0., 0.}),
+    route_nominal_center(),
     heading_diff(0.),
     route_speed_lat(0.),
     route_speed_lon(0.),
@@ -73,6 +73,12 @@ EgoDynamicsOnRoute::EgoDynamicsOnRoute()
     avg_route_accel_lat(0.),
     avg_route_accel_lon(0.) {
   timestamp.elapsed_seconds = 0.;
+  ego_center.x = 0.;
+  ego_center.y = 0.;
+  ego_center.z = 0.,
+  route_nominal_center.x = 0.;
+  route_nominal_center.y = 0.;
+  route_nominal_center.z = 0.;
 }
 
 std::shared_ptr<spdlog::logger> getLogger() {
diff --git a/LibCarla/source/test/client/test_image.cpp b/LibCarla/source/test/client/test_image.cpp
index bda3d7bb6..0d710ca43 100644
--- a/LibCarla/source/test/client/test_image.cpp
+++ b/LibCarla/source/test/client/test_image.cpp
@@ -14,7 +14,6 @@
 
 template <typename ViewT, typename PixelT>
 struct TestImage {
-  TestImage(TestImage &&) = default;
   using pixel_type = PixelT;
   std::unique_ptr<PixelT[]> data;
   ViewT view;
@@ -28,7 +27,7 @@ static auto MakeTestImage(size_t width, size_t height) {
       height,
       reinterpret_cast<PixelT*>(data.get()),
       static_cast<long>(sizeof(PixelT) * width));
-  return TestImage<decltype(view), PixelT>{std::move(data), view};
+  return TestImage<decltype(view), PixelT>{.data = std::move(data), .view = view};
 }
 
 #ifndef PLATFORM_WINDOWS
diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Carla.Build.cs b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Carla.Build.cs
index da9727d1e..b8a2fc2e8 100644
--- a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Carla.Build.cs
+++ b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Carla.Build.cs
@@ -4,25 +4,17 @@ using System;
 using System.IO;
 using UnrealBuildTool;
 
-public class Carla : ModuleRules
+
+public class Carla : CarlaRules
 {
   bool UsingCarSim = false;
   bool UsingChrono = false;
   bool UsingPytorch = false;
-  bool UsingRos2 = false;
-  private bool IsWindows(ReadOnlyTargetRules Target)
-  {
-    return (Target.Platform == UnrealTargetPlatform.Win64) || (Target.Platform == UnrealTargetPlatform.Win32);
-  }
-
-  public Carla(ReadOnlyTargetRules Target) : base(Target)
+  public Carla(ReadOnlyTargetRules Target) : base(Target, "../../CarlaDependencies")
   {
     PrivatePCHHeaderFile = "Carla.h";
-
-    if (IsWindows(Target))
-    {
-      bEnableExceptions = true;
-    }
+    bEnableExceptions = true;
+    CppStandard = CppStandardVersion.Cpp20;
 
     // Read config about carsim
     string CarlaPluginPath = Path.GetFullPath( ModuleDirectory );
@@ -57,8 +49,12 @@ public class Carla : ModuleRules
       {
         Console.WriteLine("Enabling ros2");
         UsingRos2 = true;
-        PublicDefinitions.Add("WITH_ROS2");
-        PrivateDefinitions.Add("WITH_ROS2");
+      }
+
+      if (line.Contains("Debug ON"))
+      {
+        Console.WriteLine("Enabling debug build");
+        Debug = true;
       }
     }
 
@@ -91,10 +87,10 @@ public class Carla : ModuleRules
       PublicDependencyModuleNames.AddRange(new string[] { "CarSim" });
     }
 
-	 if (Target.Type == TargetType.Editor)
-	 {
-		PublicDependencyModuleNames.AddRange(new string[] { "UnrealEd" });
-	 }
+	  if (Target.Type == TargetType.Editor)
+	  {
+      PublicDependencyModuleNames.AddRange(new string[] { "UnrealEd" });
+	  }
 
     PrivateDependencyModuleNames.AddRange(
       new string[]
@@ -118,229 +114,143 @@ public class Carla : ModuleRules
         "PhysicsCore"
         // ... add private dependencies that you statically link with here ...
       }
-      );
+    );
+
     if (UsingCarSim)
     {
       PrivateDependencyModuleNames.AddRange(new string[] { "CarSim" });
       PrivateIncludePathModuleNames.AddRange(new string[] { "CarSim" });
     }
 
-
     DynamicallyLoadedModuleNames.AddRange(
       new string[]
       {
         // ... add any modules that your module loads dynamically here ...
       }
-      );
+    );
 
-    AddCarlaServerDependency(Target);
+    AddCarlaServerDependency();
   }
 
-  private bool UseDebugLibs(ReadOnlyTargetRules Target)
+  private void AddCarlaServerDependency()
   {
-    if (IsWindows(Target))
+
+    // Link dependencies.
+    AddStaticLibrary( "rpc");
+    if (UseDebugLibs())
     {
-      // In Windows, Unreal uses the Release C++ Runtime (CRT) even in debug
-      // mode, so unless we recompile the engine we cannot link the debug
-      // libraries.
-      return false;
+      AddStaticLibrary("carla_server_debug");
     }
     else
     {
-      return false;
+      AddStaticLibrary("carla_server");
     }
-  }
-
-  private void AddDynamicLibrary(string library)
-  {
-    PublicAdditionalLibraries.Add(library);
-    RuntimeDependencies.Add(library);
-    PublicDelayLoadDLLs.Add(library);
-  }
-  private void AddDllDependency(string PathToFolder, string DllName)
-  {
-    string Source = Path.Combine(PathToFolder, DllName);
-    string Destination = Path.Combine("$(BinaryOutputDir)", DllName);
-    RuntimeDependencies.Add(Destination, Source);
-  }
-
-  delegate string ADelegate(string s);
-
-  private void AddBoostLibs(string LibPath)
-  {
-    string [] files = Directory.GetFiles(LibPath, "*boost*.lib");
-    foreach (string file in files) PublicAdditionalLibraries.Add(file);
-  }
-
-  private void AddCarlaServerDependency(ReadOnlyTargetRules Target)
-  {
-    string LibCarlaInstallPath = Path.GetFullPath(Path.Combine(ModuleDirectory, "../../CarlaDependencies"));
-
-    ADelegate GetLibName = (string BaseName) => {
-      if (IsWindows(Target))
-      {
-        return BaseName + ".lib";
-      }
-      else
-      {
-        return "lib" + BaseName + ".a";
-      }
-    };
-
-    // Link dependencies.
-    if (IsWindows(Target))
+    if (IsWindows())
     {
-      AddBoostLibs(Path.Combine(LibCarlaInstallPath, "lib"));
-      PublicAdditionalLibraries.Add(Path.Combine(LibCarlaInstallPath, "lib", GetLibName("rpc")));
+       PublicAdditionalLibraries.Add("shlwapi.lib");
 
-      if (UseDebugLibs(Target))
-      {
-        PublicAdditionalLibraries.Add(Path.Combine(LibCarlaInstallPath, "lib", GetLibName("carla_server_debug")));
-      }
-      else
-      {
-        PublicAdditionalLibraries.Add(Path.Combine(LibCarlaInstallPath, "lib", GetLibName("carla_server")));
-      }
+      AddBoostLibs();
       if (UsingChrono)
       {
-        PublicAdditionalLibraries.Add(Path.Combine(LibCarlaInstallPath, "lib", GetLibName("ChronoEngine")));
-        PublicAdditionalLibraries.Add(Path.Combine(LibCarlaInstallPath, "lib", GetLibName("ChronoEngine_vehicle")));
-        PublicAdditionalLibraries.Add(Path.Combine(LibCarlaInstallPath, "lib", GetLibName("ChronoModels_vehicle")));
-        PublicAdditionalLibraries.Add(Path.Combine(LibCarlaInstallPath, "lib", GetLibName("ChronoModels_robot")));
+        AddStaticLibrary("ChronoEngine");
+        AddStaticLibrary("ChronoEngine_vehicle");
+        AddStaticLibrary("ChronoModels_vehicle");
+        AddStaticLibrary("ChronoModels_robot");
         AddDllDependency(Path.Combine(LibCarlaInstallPath, "dll"), "ChronoEngine.dll");
         AddDllDependency(Path.Combine(LibCarlaInstallPath, "dll"), "ChronoEngine_vehicle.dll");
         AddDllDependency(Path.Combine(LibCarlaInstallPath, "dll"), "ChronoModels_vehicle.dll");
         AddDllDependency(Path.Combine(LibCarlaInstallPath, "dll"), "ChronoModels_robot.dll");
         bUseRTTI = true;
       }
-
-      //OsmToODR
-      PublicAdditionalLibraries.Add(Path.Combine(LibCarlaInstallPath, "lib", "sqlite3.lib"));
-      PublicAdditionalLibraries.Add(Path.Combine(LibCarlaInstallPath, "lib", "xerces-c_3.lib"));
-      PublicAdditionalLibraries.Add(Path.Combine(LibCarlaInstallPath, "lib", "proj.lib"));
-      PublicAdditionalLibraries.Add(Path.Combine(LibCarlaInstallPath, "lib", "osm2odr.lib"));
-      PublicAdditionalLibraries.Add(Path.Combine(LibCarlaInstallPath, "lib", "zlibstatic.lib"));
     }
     else
     {
-      PublicAdditionalLibraries.Add(Path.Combine(LibCarlaInstallPath, "lib", GetLibName("rpc")));
-      if (UseDebugLibs(Target))
-      {
-        PublicAdditionalLibraries.Add(Path.Combine(LibCarlaInstallPath, "lib", GetLibName("carla_server_debug")));
-      }
-      else
-      {
-        PublicAdditionalLibraries.Add(Path.Combine(LibCarlaInstallPath, "lib", GetLibName("carla_server")));
-      }
       if (UsingChrono)
       {
-        AddDynamicLibrary(Path.Combine(LibCarlaInstallPath, "lib", "libChronoEngine.so"));
-        AddDynamicLibrary(Path.Combine(LibCarlaInstallPath, "lib", "libChronoEngine_vehicle.so"));
-        AddDynamicLibrary(Path.Combine(LibCarlaInstallPath, "lib", "libChronoModels_vehicle.so"));
-        AddDynamicLibrary(Path.Combine(LibCarlaInstallPath, "lib", "libChronoModels_robot.so"));
+        AddStaticLibrary( "ChronoEngine");
+        AddStaticLibrary( "ChronoEngine_vehicle");
+        AddStaticLibrary( "ChronoModels_vehicle");
+        AddStaticLibrary( "ChronoModels_robot");
 
         bUseRTTI = true;
       }
 
       if (UsingPytorch)
       {
-        PublicAdditionalLibraries.Add(Path.Combine(LibCarlaInstallPath, "lib", GetLibName("carla_pytorch")));
+        AddStaticLibrary( "carla_pytorch");
 
-        string LibTorchPath = LibCarlaInstallPath;
-        PublicAdditionalLibraries.Add(Path.Combine(LibTorchPath, "lib", "libonnx_proto.a"));
-        PublicAdditionalLibraries.Add(Path.Combine(LibTorchPath, "lib", "libfbgemm.a"));
-        PublicAdditionalLibraries.Add(Path.Combine(LibTorchPath, "lib", "libgloo.a"));
-        PublicAdditionalLibraries.Add(Path.Combine(LibTorchPath, "lib", "libXNNPACK.a"));
-        PublicAdditionalLibraries.Add(Path.Combine(LibTorchPath, "lib", "libprotobuf-lite.a"));
-        PublicAdditionalLibraries.Add(Path.Combine(LibTorchPath, "lib", "libprotobuf.a"));
-        PublicAdditionalLibraries.Add(Path.Combine(LibTorchPath, "lib", "libasmjit.a"));
-        PublicAdditionalLibraries.Add(Path.Combine(LibTorchPath, "lib", "libcpuinfo_internals.a"));
-        PublicAdditionalLibraries.Add(Path.Combine(LibTorchPath, "lib", "libclog.a"));
-        // PublicAdditionalLibraries.Add(Path.Combine(LibTorchPath, "lib", "libbreakpad_common.a"));
-        PublicAdditionalLibraries.Add(Path.Combine(LibTorchPath, "lib", "libbenchmark.a"));
-        PublicAdditionalLibraries.Add(Path.Combine(LibTorchPath, "lib", "libtensorpipe.a"));
-        PublicAdditionalLibraries.Add(Path.Combine(LibTorchPath, "lib", "libpytorch_qnnpack.a"));
-        PublicAdditionalLibraries.Add(Path.Combine(LibTorchPath, "lib", "libtensorpipe_cuda.a"));
-        PublicAdditionalLibraries.Add(Path.Combine(LibTorchPath, "lib", "libnnpack_reference_layers.a"));
-        PublicAdditionalLibraries.Add(Path.Combine(LibTorchPath, "lib", "libgmock.a"));
-        PublicAdditionalLibraries.Add(Path.Combine(LibTorchPath, "lib", "libdnnl.a"));
-        PublicAdditionalLibraries.Add(Path.Combine(LibTorchPath, "lib", "libpthreadpool.a"));
-        PublicAdditionalLibraries.Add(Path.Combine(LibTorchPath, "lib", "libcpuinfo.a"));
-        PublicAdditionalLibraries.Add(Path.Combine(LibTorchPath, "lib", "libqnnpack.a"));
-        // PublicAdditionalLibraries.Add(Path.Combine(LibTorchPath, "lib", "libbreakpad.a"));
-        PublicAdditionalLibraries.Add(Path.Combine(LibTorchPath, "lib", "libkineto.a"));
-        PublicAdditionalLibraries.Add(Path.Combine(LibTorchPath, "lib", "libprotoc.a"));
-        PublicAdditionalLibraries.Add(Path.Combine(LibTorchPath, "lib", "libgtest.a"));
-        PublicAdditionalLibraries.Add(Path.Combine(LibTorchPath, "lib", "libgmock_main.a"));
-        PublicAdditionalLibraries.Add(Path.Combine(LibTorchPath, "lib", "libgtest_main.a"));
-        PublicAdditionalLibraries.Add(Path.Combine(LibTorchPath, "lib", "libbenchmark_main.a"));
-        PublicAdditionalLibraries.Add(Path.Combine(LibTorchPath, "lib", "libfmt.a"));
-        PublicAdditionalLibraries.Add(Path.Combine(LibTorchPath, "lib", "libtensorpipe_uv.a"));
-        PublicAdditionalLibraries.Add(Path.Combine(LibTorchPath, "lib", "libfoxi_loader.a"));
-        PublicAdditionalLibraries.Add(Path.Combine(LibTorchPath, "lib", "libgloo_cuda.a"));
-        PublicAdditionalLibraries.Add(Path.Combine(LibTorchPath, "lib", "libnnpack.a"));
-        PublicAdditionalLibraries.Add(Path.Combine(LibTorchPath, "lib", "libcaffe2_protos.a"));
-        PublicAdditionalLibraries.Add(Path.Combine(LibTorchPath, "lib", "libonnx.a"));
-        AddDynamicLibrary(Path.Combine(LibTorchPath, "lib", "libtorch.so"));
-        AddDynamicLibrary(Path.Combine(LibTorchPath, "lib", "libnnapi_backend.so"));
-        AddDynamicLibrary(Path.Combine(LibTorchPath, "lib", "libbackend_with_compiler.so"));
-        AddDynamicLibrary(Path.Combine(LibTorchPath, "lib", "libcaffe2_nvrtc.so"));
-        AddDynamicLibrary(Path.Combine(LibTorchPath, "lib", "libtorch_cuda_cpp.so"));
-        AddDynamicLibrary(Path.Combine(LibTorchPath, "lib", "libc10_cuda.so"));
-        AddDynamicLibrary(Path.Combine(LibTorchPath, "lib", "libtorchbind_test.so"));
-        AddDynamicLibrary(Path.Combine(LibTorchPath, "lib", "libjitbackend_test.so"));
-        AddDynamicLibrary(Path.Combine(LibTorchPath, "lib", "libc10.so"));
-        AddDynamicLibrary(Path.Combine(LibTorchPath, "lib", "libtorch_cuda.so"));
-        AddDynamicLibrary(Path.Combine(LibTorchPath, "lib", "libtorch_global_deps.so"));
-        AddDynamicLibrary(Path.Combine(LibTorchPath, "lib", "libtorch_cpu.so"));
-        AddDynamicLibrary(Path.Combine(LibTorchPath, "lib", "libshm.so"));
-        AddDynamicLibrary(Path.Combine(LibTorchPath, "lib", "libtorch_cuda_cu.so"));
-        AddDynamicLibrary(Path.Combine(LibTorchPath, "lib", "libtorchscatter.so"));
-        AddDynamicLibrary(Path.Combine(LibTorchPath, "lib", "libtorchcluster.so"));
-        // AddDynamicLibrary("/usr/local/cuda/lib64/stubs/libcuda.so");
-        // AddDynamicLibrary("/usr/local/cuda/lib64/libnvrtc.so");
-        // AddDynamicLibrary("/usr/local/cuda/lib64/libnvToolsExt.so");
-        // AddDynamicLibrary("/usr/local/cuda/lib64/libcudart.so");
-        // AddDynamicLibrary("/usr/lib/llvm-10/lib/libgomp.so");
-        PublicAdditionalLibraries.Add("/usr/local/cuda/lib64/stubs/libcuda.so");
-        PublicAdditionalLibraries.Add("/usr/local/cuda/lib64/libnvrtc.so");
-        PublicAdditionalLibraries.Add("/usr/local/cuda/lib64/libnvToolsExt.so");
-        PublicAdditionalLibraries.Add("/usr/local/cuda/lib64/libcudart.so");
-        PublicAdditionalLibraries.Add("/usr/lib/llvm-10/lib/libgomp.so");
-        RuntimeDependencies.Add(Path.Combine(LibTorchPath, "lib", "libcudart-a7b20f20.so.11.0"));
-        RuntimeDependencies.Add(Path.Combine(LibTorchPath, "lib", "libgomp-a34b3233.so.1"));
-        RuntimeDependencies.Add(Path.Combine(LibTorchPath, "lib", "libnvrtc-builtins-4730a239.so.11.3"));
-        RuntimeDependencies.Add(Path.Combine(LibTorchPath, "lib", "libnvrtc-1ea278b5.so.11.2"));
-        RuntimeDependencies.Add(Path.Combine(LibTorchPath, "lib", "libnvToolsExt-24de1d56.so.1"));
+        AddStaticLibrary("onnx_proto");
+        AddStaticLibrary("fbgemm");
+        AddStaticLibrary("gloo");
+        AddStaticLibrary("XNNPACK");
+        AddStaticLibrary("protobuf-lite");
+        AddStaticLibrary("protobuf");
+        AddStaticLibrary("asmjit");
+        AddStaticLibrary("cpuinfo_internals");
+        AddStaticLibrary("clog");
+        // AddStaticLibrary("breakpad_common");
+        AddStaticLibrary("benchmark");
+        AddStaticLibrary("tensorpipe");
+        AddStaticLibrary("pytorch_qnnpack");
+        AddStaticLibrary("tensorpipe_cuda");
+        AddStaticLibrary("nnpack_reference_layers");
+        AddStaticLibrary("gmock");
+        AddStaticLibrary("dnnl");
+        AddStaticLibrary("pthreadpool");
+        AddStaticLibrary("cpuinfo");
+        AddStaticLibrary("qnnpack");
+        // AddStaticLibrary("breakpad"));
+        AddStaticLibrary("kineto");
+        AddStaticLibrary("protoc");
+        AddStaticLibrary("gtest");
+        AddStaticLibrary("gmock_main");
+        AddStaticLibrary("gtest_main");
+        AddStaticLibrary("benchmark_main");
+        AddStaticLibrary("fmt");
+        AddStaticLibrary("tensorpipe_uv");
+        AddStaticLibrary("foxi_loader");
+        AddStaticLibrary("gloo_cuda");
+        AddStaticLibrary("nnpack");
+        AddStaticLibrary("caffe2_protos");
+        AddStaticLibrary("onnx");
+        AddDynamicLibrary("torch");
+        AddDynamicLibrary("nnapi_backend");
+        AddDynamicLibrary("backend_with_compiler");
+        AddDynamicLibrary("caffe2_nvrtc");
+        AddDynamicLibrary("torch_cuda_cpp");
+        AddDynamicLibrary("c10_cuda");
+        AddDynamicLibrary("torchbind_test");
+        AddDynamicLibrary("jitbackend_test");
+        AddDynamicLibrary("c10");
+        AddDynamicLibrary("torch_cuda");
+        AddDynamicLibrary("torch_global_deps");
+        AddDynamicLibrary("torch_cpu");
+        AddDynamicLibrary("shm");
+        AddDynamicLibrary("torch_cuda_cu");
+        AddDynamicLibrary("torchscatter");
+        AddDynamicLibrary("torchcluster");
+        AddDynamicLibrary("cuda", "/usr/local/cuda/lib64/stubs");
+        AddDynamicLibrary("nvrtc", "/usr/local/cuda/lib64");
+        AddDynamicLibrary("nvToolsExt", "/usr/local/cuda/lib64");
+        AddDynamicLibrary("cudart", "/usr/local/cuda/lib64");
+        AddDynamicLibrary("gomp", "/usr/lib/llvm-10/lib");
+        RuntimeDependencies.Add(Path.Combine("libcudart-a7b20f20.so.11.0"));
+        RuntimeDependencies.Add(Path.Combine("libgomp-a34b3233.so.1"));
+        RuntimeDependencies.Add(Path.Combine("libnvrtc-builtins-4730a239.so.11.3"));
+        RuntimeDependencies.Add(Path.Combine("libnvrtc-1ea278b5.so.11.2"));
+        RuntimeDependencies.Add(Path.Combine("libnvToolsExt-24de1d56.so.1"));
         PublicAdditionalLibraries.Add("stdc++");
         PublicAdditionalLibraries.Add("/usr/lib/x86_64-linux-gnu/libpython3.9.so");
       }
-
-      if (UsingRos2)
-      {
-        PublicAdditionalLibraries.Add(Path.Combine(LibCarlaInstallPath, "lib", GetLibName("carla_fastdds")));
-
-        string LibFastDDSPath = LibCarlaInstallPath;
-        AddDynamicLibrary(Path.Combine(LibFastDDSPath, "lib", "libfoonathan_memory-0.7.3.so"));
-        AddDynamicLibrary(Path.Combine(LibFastDDSPath, "lib", "libfastcdr.so"));
-        AddDynamicLibrary(Path.Combine(LibFastDDSPath, "lib", "libfastrtps.so"));
-        PublicAdditionalLibraries.Add("stdc++");
-      }
-
-
-      //OsmToODR
-      PublicAdditionalLibraries.Add("/usr/lib/x86_64-linux-gnu/libc.so");
-      PublicAdditionalLibraries.Add(Path.Combine(LibCarlaInstallPath, "lib", "libsqlite3.so"));
-      PublicAdditionalLibraries.Add(Path.Combine(LibCarlaInstallPath, "lib", "libxerces-c.a"));
-      PublicAdditionalLibraries.Add(Path.Combine(LibCarlaInstallPath, "lib", "libproj.a"));
-      PublicAdditionalLibraries.Add(Path.Combine(LibCarlaInstallPath, "lib", "libosm2odr.a"));
-
     }
-    bEnableExceptions = true;
+
+    addOsmToODR();
+
+    addROS2();
+
 
     // Include path.
     string LibCarlaIncludePath = Path.Combine(LibCarlaInstallPath, "include");
-
     PublicIncludePaths.Add(LibCarlaIncludePath);
     PrivateIncludePaths.Add(LibCarlaIncludePath);
 
diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/CarlaRules.Build.cs b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/CarlaRules.Build.cs
new file mode 100644
index 000000000..4a4206a67
--- /dev/null
+++ b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/CarlaRules.Build.cs
@@ -0,0 +1,179 @@
+// Copyright 1998-2017 Epic Games, Inc. All Rights Reserved.
+
+using System;
+using System.IO;
+using UnrealBuildTool;
+
+public class CarlaRules : ModuleRules
+{
+  protected string LibCarlaInstallPath;
+  protected bool UsingRos2 = false;
+  protected bool Debug = false;
+
+  public CarlaRules(ReadOnlyTargetRules Target, string LibCarlaInstallPathRelativeToModule) : base(Target)
+  {
+    LibCarlaInstallPath = Path.GetFullPath(Path.Combine(ModuleDirectory, LibCarlaInstallPathRelativeToModule));
+  }
+
+  protected bool IsWindows()
+  {
+    return (Target.Platform == UnrealTargetPlatform.Win64) || (Target.Platform == UnrealTargetPlatform.Win32);
+  }
+
+  protected bool IsLinux()
+  {
+    return (Target.Platform == UnrealTargetPlatform.Linux) || (Target.Platform == UnrealTargetPlatform.LinuxAArch64);
+  }
+
+  protected bool IsMac()
+  {
+    return (Target.Platform == UnrealTargetPlatform.Mac);
+  }
+
+  protected bool UseDebugLibs()
+  {
+    if (IsWindows())
+    {
+      // In Windows, Unreal uses the Release C++ Runtime (CRT) even in debug
+      // mode, so unless we recompile the engine we cannot link the debug
+      // libraries.
+      return false;
+    }
+    else
+    {
+      return Debug;
+    }
+  }
+
+  protected void AddDllDependency(string PathToFolder, string DllName)
+  {
+    string Source = Path.Combine(PathToFolder, DllName);
+    string Destination = Path.Combine("$(BinaryOutputDir)", DllName);
+    RuntimeDependencies.Add(Destination, Source);
+  }
+
+  protected void AddBoostLibs()
+  {
+    string [] files = Directory.GetFiles(Path.Combine(LibCarlaInstallPath, "lib"), "*boost*.lib");
+    foreach (string file in files) PublicAdditionalLibraries.Add(file);
+  }
+
+  protected void AddStaticLibrary(string LibBaseName, string LinkBasePath = "", string LibPrefix = "", string LibPostfix = "") {
+    string linkBasePath = LinkBasePath;
+    if ( linkBasePath == "" )
+    {
+      linkBasePath = Path.GetFullPath(Path.Combine(LibCarlaInstallPath, "lib"));
+    }
+
+    string libPrefix = LibPrefix;
+    if ( libPrefix == "" )
+    {
+      if (IsWindows())
+      {
+        libPrefix = "";
+      }
+      else
+      {
+        libPrefix = "lib";
+      }
+    }
+
+    string libPostfix = LibPostfix;
+    if ( libPostfix == "" )
+    {
+      if (IsWindows())
+      {
+        libPostfix = ".lib";
+      }
+      else
+      {
+        libPostfix = ".a";
+      }
+    }
+
+    string linkLibrary= Path.Combine(linkBasePath, libPrefix + LibBaseName + libPostfix);
+    PublicAdditionalLibraries.Add(linkLibrary);
+  }
+  protected void AddDynamicLibrary(string LibBaseName, string LinkBasePath = "", string LibPrefix = "", string LibPostfix = "")
+  {
+    string linkBasePath = LinkBasePath;
+    if ( linkBasePath == "" )
+    {
+      linkBasePath = Path.GetFullPath(Path.Combine(LibCarlaInstallPath, "lib"));
+    }
+
+    string libPrefix;
+    if (IsWindows())
+    {
+      libPrefix = "";
+    }
+    else
+    {
+      libPrefix = "lib";
+    }
+    if ( LibPrefix != "" )
+    {
+      libPrefix = LibPrefix;
+    }
+
+    string libPostfix;
+    if (IsWindows())
+    {
+      libPostfix = ".dll";
+    }
+    else
+    {
+      libPostfix = ".so";
+    }
+    if (LibPostfix != "")
+    {
+      libPostfix = LibPostfix;
+    }
+
+    string linkLibrary= Path.Combine(linkBasePath, libPrefix + LibBaseName + libPostfix);
+    PublicAdditionalLibraries.Add(linkLibrary);
+    RuntimeDependencies.Add(linkLibrary);
+    if ( IsWindows() )
+    {
+      PublicDelayLoadDLLs.Add(linkLibrary);
+    }
+  }
+
+  protected void addOsmToODR()
+  {
+      AddStaticLibrary( "sqlite3");
+      AddStaticLibrary( "proj");
+      AddStaticLibrary( "osm2odr");
+      if ( IsWindows() )
+      {
+        AddStaticLibrary( "xerces-c_3");
+        AddStaticLibrary( "zlibstatic");
+      }
+      else
+      {
+        AddStaticLibrary( "xerces-c");
+      }
+  }
+
+  protected void addROS2()
+  {
+    if (UsingRos2) 
+    {
+      AddStaticLibrary( "foonathan_memory-0.7.3");
+      if ( IsWindows() )
+      {
+        AddStaticLibrary( "libfastcdr-1.1");
+        AddStaticLibrary( "libfastrtps-2.11");
+      }
+      else
+      {
+        AddStaticLibrary( "fastcdr");
+        AddStaticLibrary( "fastrtps");
+      }
+      PublicIncludePaths.Add(Path.Combine(LibCarlaInstallPath, "include", "carla", "ros2", "ros_types"));
+      PrivateIncludePaths.Add(Path.Combine(LibCarlaInstallPath, "include", "carla", "ros2", "ros_types"));
+      PublicDefinitions.Add("WITH_ROS2");
+      PrivateDefinitions.Add("WITH_ROS2");
+    }
+  }
+}
diff --git a/Unreal/CarlaUE4/Plugins/CarlaTools/Source/CarlaTools/CarlaTools.Build.cs b/Unreal/CarlaUE4/Plugins/CarlaTools/Source/CarlaTools/CarlaTools.Build.cs
index a5181554a..230f2ef15 100644
--- a/Unreal/CarlaUE4/Plugins/CarlaTools/Source/CarlaTools/CarlaTools.Build.cs
+++ b/Unreal/CarlaUE4/Plugins/CarlaTools/Source/CarlaTools/CarlaTools.Build.cs
@@ -4,21 +4,17 @@ using System;
 using System.IO;
 using UnrealBuildTool;
 
-public class CarlaTools : ModuleRules
+public class CarlaTools : CarlaRules
 {
   bool bUsingOmniverseConnector = false;
-  private bool IsWindows(ReadOnlyTargetRules Target)
-  {
-    return (Target.Platform == UnrealTargetPlatform.Win64) || (Target.Platform == UnrealTargetPlatform.Win32);
-  }
 
-	public CarlaTools(ReadOnlyTargetRules Target) : base(Target)
+	public CarlaTools(ReadOnlyTargetRules Target) : base(Target, "../../../Carla/CarlaDependencies")
 	{
 		PCHUsage = ModuleRules.PCHUsageMode.UseExplicitOrSharedPCHs;
 
     // PrivatePCHHeaderFile = "Carla.h";
 
-    if (IsWindows(Target))
+    if (IsWindows())
     {
       bEnableExceptions = true;
     }
@@ -36,6 +32,13 @@ public class CarlaTools : ModuleRules
         PublicDefinitions.Add("WITH_OMNIVERSE");
         PrivateDefinitions.Add("WITH_OMNIVERSE");
       }
+
+      if (line.Contains("Ros2 ON"))
+      {
+        Console.WriteLine("Enabling ros2");
+        UsingRos2 = true;
+      }
+
     }
 
 		PublicIncludePaths.AddRange(
@@ -112,78 +115,32 @@ public class CarlaTools : ModuleRules
 				// ... add any modules that your module loads dynamically here ...
 			}
 			);
-		AddCarlaServerDependency(Target);
+		AddCarlaServerDependency();
 	}
 
-  private bool UseDebugLibs(ReadOnlyTargetRules Target)
-  {
-    if (IsWindows(Target))
-    {
-      // In Windows, Unreal uses the Release C++ Runtime (CRT) even in debug
-      // mode, so unless we recompile the engine we cannot link the debug
-      // libraries.
-      return false;
-    }
-    else
-    {
-      return false;
-    }
-  }
-
-  delegate string ADelegate(string s);
-
-  private void AddBoostLibs(string LibPath)
-  {
-    string [] files = Directory.GetFiles(LibPath, "*boost*.lib");
-    foreach (string file in files)
-    {
-      PublicAdditionalLibraries.Add(file);
-    }
-  }
-
-
-	private void AddCarlaServerDependency(ReadOnlyTargetRules Target)
+  
+	private void AddCarlaServerDependency()
 	{
-		string LibCarlaInstallPath = Path.GetFullPath(Path.Combine(ModuleDirectory, "../../../Carla/CarlaDependencies"));
-
-		ADelegate GetLibName = (string BaseName) => {
-			if (IsWindows(Target))
-			{
-				return BaseName + ".lib";
-			}
-			else
-			{
-				return "lib" + BaseName + ".a";
-			}
-		};
 
     // Link dependencies.
-    if (IsWindows(Target))
+    if (IsWindows())
     {
-      AddBoostLibs(Path.Combine(LibCarlaInstallPath, "lib"));
-      PublicAdditionalLibraries.Add(Path.Combine(LibCarlaInstallPath, "lib", GetLibName("rpc")));
-
-      if (UseDebugLibs(Target))
-      {
-        PublicAdditionalLibraries.Add(Path.Combine(LibCarlaInstallPath, "lib", GetLibName("carla_server_debug")));
-      }
-      else
-      {
-        PublicAdditionalLibraries.Add(Path.Combine(LibCarlaInstallPath, "lib", GetLibName("carla_server")));
-      }
+      AddBoostLibs();
+    }
+    AddStaticLibrary("rpc");
+    if (UseDebugLibs())
+    {
+      AddStaticLibrary("carla_server_debug");
     }
     else
     {
-      PublicAdditionalLibraries.Add(Path.Combine(LibCarlaInstallPath, "lib", GetLibName("rpc")));
-      if (UseDebugLibs(Target))
-      {
-        PublicAdditionalLibraries.Add(Path.Combine(LibCarlaInstallPath, "lib", GetLibName("carla_server_debug")));
-      }
-      else
-      {
-        PublicAdditionalLibraries.Add(Path.Combine(LibCarlaInstallPath, "lib", GetLibName("carla_server")));
-      }
+      AddStaticLibrary("carla_server");
     }
+
+    addOsmToODR();
+
+    addROS2();
+
     // Include path.
     string LibCarlaIncludePath = Path.Combine(LibCarlaInstallPath, "include");
 
@@ -192,7 +149,7 @@ public class CarlaTools : ModuleRules
 
     PublicDefinitions.Add("ASIO_NO_EXCEPTIONS");
     PublicDefinitions.Add("BOOST_NO_EXCEPTIONS");
-    // PublicDefinitions.Add("LIBCARLA_NO_EXCEPTIONS");
+    PublicDefinitions.Add("LIBCARLA_NO_EXCEPTIONS");
     PublicDefinitions.Add("PUGIXML_NO_EXCEPTIONS");
     PublicDefinitions.Add("BOOST_DISABLE_ABI_HEADERS");
     PublicDefinitions.Add("BOOST_TYPE_INDEX_FORCE_NO_RTTI_COMPATIBILITY");
diff --git a/Util/BuildTools/Ad-rss.sh b/Util/BuildTools/Ad-rss.sh
index 3f7c71695..f4c978e60 100755
--- a/Util/BuildTools/Ad-rss.sh
+++ b/Util/BuildTools/Ad-rss.sh
@@ -22,6 +22,13 @@ done
 # ==============================================================================
 source $(dirname "$0")/Environment.sh
 
+
+UNREAL_SYSROOT="$UE4_ROOT/Engine/Extras/ThirdPartyNotUE/SDKs/HostLinux/Linux_x64/v17_clang-10.0.1-centos7/x86_64-unknown-linux-gnu"
+export CC="${UNREAL_SYSROOT}/bin/clang"
+export CXX="${UNREAL_SYSROOT}/bin/clang++"
+export PATH="${UNREAL_SYSROOT}/bin:$PATH"
+
+
 # Convert comma-separated string to array of unique elements.
 IFS="," read -r -a PY_VERSION_LIST <<< "${PY_VERSION_LIST}"
 
@@ -29,7 +36,7 @@ IFS="," read -r -a PY_VERSION_LIST <<< "${PY_VERSION_LIST}"
 # -- Get ad-rss -------------------------------------------
 # ==============================================================================
 
-ADRSS_VERSION=4.4.4
+ADRSS_VERSION=4.4.5
 ADRSS_BASENAME=ad-rss-${ADRSS_VERSION}
 ADRSS_COLCON_WORKSPACE="${CARLA_BUILD_FOLDER}/${ADRSS_BASENAME}"
 ADRSS_SRC_DIR="${ADRSS_COLCON_WORKSPACE}/src"
@@ -42,11 +49,8 @@ if [[ ! -d "${ADRSS_SRC_DIR}" ]]; then
 
   # clone ad-rss with all submodules, but remove proj, as CARLA already uses it
   pushd "${ADRSS_SRC_DIR}" >/dev/null
-  git clone -b v${ADRSS_VERSION} https://github.com/intel/ad-rss-lib.git && cd ad-rss-lib && git submodule update --init --recursive && rm -rf dependencies/map/dependencies/PROJ4 && cd ..
+  git clone -b v${ADRSS_VERSION} https://github.com/intel/ad-rss-lib.git && cd ad-rss-lib && git submodule update --init --recursive && cd ..
 
-  # ADRSS_VERSION is designed for older boost, update datatype from boost::array to std::array
-  grep -rl "boost::array" | xargs sed -i 's/boost::array/std::array/g'
-  grep -rl "find_package(Boost" | xargs sed -i 's/find_package(Boost/find_package(Boost 1.80/g'
   popd
 
   cat >"${ADRSS_COLCON_WORKSPACE}/colcon.meta" <<EOL
@@ -91,8 +95,6 @@ ADRSS_INSTALL_DIR="${CARLA_BUILD_FOLDER}/${ADRSS_BASENAME}/install"
 #else
 #  echo "Using clang-$CARLA_LLVM_VERSION_MAJOR as the CARLA compiler."
 #fi
-CXX_TAG=c10
-
 #
 # Since it it not possible with boost-python to build more than one python version at once (find_package has some bugs)
 # we have to build it for every version in a separate colcon build
@@ -107,9 +109,9 @@ for PY_VERSION in ${PY_VERSION_LIST[@]} ; do
 
     pushd "${ADRSS_COLCON_WORKSPACE}" >/dev/null
     if [[ "${CMAKE_PREFIX_PATH}" == "" ]]; then
-      CMAKE_PREFIX_PATH="${CARLA_BUILD_FOLDER}/boost-1.80.0-$CXX_TAG-install;${CARLA_BUILD_FOLDER}/proj-install"
+      CMAKE_PREFIX_PATH="${CARLA_BUILD_FOLDER}/boost-1.80.0-client-install;${CARLA_BUILD_FOLDER}/proj-client-install"
     else
-      CMAKE_PREFIX_PATH="${CARLA_BUILD_FOLDER}/boost-1.80.0-$CXX_TAG-install;${CARLA_BUILD_FOLDER}/proj-install;${CMAKE_PREFIX_PATH}"
+      CMAKE_PREFIX_PATH="${CARLA_BUILD_FOLDER}/boost-1.80.0-client-install;${CARLA_BUILD_FOLDER}/proj-client-install;${CMAKE_PREFIX_PATH}"
     fi
 
     # get the python version of the binding to be built, need to query the binary,
@@ -120,7 +122,7 @@ for PY_VERSION in ${PY_VERSION_LIST[@]} ; do
     echo "PYTHON_BINDING_VERSIONS=${PYTHON_BINDING_VERSIONS}"
 
     # enforce sequential executor to reduce the required memory for compilation
-    colcon build --executor sequential --packages-up-to ad_rss_map_integration --cmake-args -DCMAKE_BUILD_TYPE=RelWithDebInfo -DCMAKE_TOOLCHAIN_FILE="${CARLA_BUILD_FOLDER}/LibStdCppToolChain.cmake" -DCMAKE_PREFIX_PATH="${CMAKE_PREFIX_PATH}" -DPYTHON_BINDING_VERSIONS="${PYTHON_BINDING_VERSIONS}" --build-base ${ADRSS_BUILD_DIR} --install-base ${ADRSS_INSTALL_DIR}
+    colcon build --executor sequential --packages-up-to ad_rss_map_integration --cmake-args -DCMAKE_BUILD_TYPE=RelWithDebInfo -DCMAKE_TOOLCHAIN_FILE="${CARLA_BUILD_FOLDER}/CarlaClientToolChain.cmake" -DCMAKE_PREFIX_PATH="${CMAKE_PREFIX_PATH}" -DPYTHON_BINDING_VERSIONS="${PYTHON_BINDING_VERSIONS}" --build-base ${ADRSS_BUILD_DIR} --install-base ${ADRSS_INSTALL_DIR}
 
     COLCON_RESULT=$?
     if (( COLCON_RESULT )); then
diff --git a/Util/BuildTools/BuildCarlaUE4.sh b/Util/BuildTools/BuildCarlaUE4.sh
index 4db570edb..8212fa960 100755
--- a/Util/BuildTools/BuildCarlaUE4.sh
+++ b/Util/BuildTools/BuildCarlaUE4.sh
@@ -6,7 +6,7 @@
 
 DOC_STRING="Build and launch CarlaUE4."
 
-USAGE_STRING="Usage: $0 [-h|--help] [--build] [--rebuild] [--launch] [--clean] [--hard-clean] [--opengl]"
+USAGE_STRING="Usage: $0 [-h|--help] [--build] [--rebuild] [--launch] [--clean] [--hard-clean] [--opengl] [--ros2]"
 
 REMOVE_INTERMEDIATE=false
 HARD_CLEAN=false
@@ -17,11 +17,13 @@ USE_CHRONO=false
 USE_PYTORCH=false
 USE_UNITY=true
 USE_ROS2=false
+EDITOR_ROS2_FLAGS=""
 
 EDITOR_FLAGS=""
 
 GDB=
 RHI="-vulkan"
+DEBUG=false
 
 OPTS=`getopt -o h --long help,build,rebuild,launch,clean,hard-clean,gdb,opengl,carsim,pytorch,chrono,ros2,no-unity,editor-flags: -n 'parse-options' -- "$@"`
 
@@ -35,6 +37,9 @@ while [[ $# -gt 0 ]]; do
     --gdb )
       GDB="gdb --args";
       shift ;;
+    --debug )
+      DEBUG=true;
+      shift ;;
     --build )
       BUILD_CARLAUE4=true;
       shift ;;
@@ -66,6 +71,9 @@ while [[ $# -gt 0 ]]; do
       shift ;;
     --ros2 )
       USE_ROS2=true;
+      # Due to continued segfaults in reallocations of MallocBinned2 enforce using AnsiMalloc calls 
+      # (see https://forums.unrealengine.com/t/dealing-with-allocator-mismatches-with-external-libraries/1416830)
+      EDITOR_ROS2_FLAGS="-ansimalloc"
       shift ;;
     --no-unity )
       USE_UNITY=false
@@ -110,7 +118,7 @@ if ${HARD_CLEAN} ; then
 
   log "Doing a \"hard\" clean of the Unreal Engine project."
 
-  make CarlaUE4Editor ARGS=-clean
+  make CarlaUE4Editor ARGS="-clean"
 
 fi
 
@@ -122,16 +130,16 @@ if ${REMOVE_INTERMEDIATE} ; then
 
   rm -Rf ${UE4_INTERMEDIATE_FOLDERS}
 
+  cd Plugins
+  rm -Rf HoudiniEngine
+  cd ..
+
   rm -f Makefile
 
   pushd "${CARLAUE4_PLUGIN_ROOT_FOLDER}" >/dev/null
 
   rm -Rf ${UE4_INTERMEDIATE_FOLDERS}
 
-  cd Plugins
-  rm -Rf HoudiniEngine
-  cd ..
-
   popd >/dev/null
 
 fi
@@ -165,12 +173,16 @@ if ${BUILD_CARLAUE4} ; then
   else
     OPTIONAL_MODULES_TEXT="Ros2 OFF"$'\n'"${OPTIONAL_MODULES_TEXT}"
   fi
+  if ${DEBUG} ; then
+    OPTIONAL_MODULES_TEXT="Debug ON"$'\n'"${OPTIONAL_MODULES_TEXT}"
+  else
+    OPTIONAL_MODULES_TEXT="Debug OFF"$'\n'"${OPTIONAL_MODULES_TEXT}"
+  fi
   if ${USE_UNITY} ; then
     OPTIONAL_MODULES_TEXT="Unity ON"$'\n'"${OPTIONAL_MODULES_TEXT}"
   else
     OPTIONAL_MODULES_TEXT="Unity OFF"$'\n'"${OPTIONAL_MODULES_TEXT}"
   fi
-  OPTIONAL_MODULES_TEXT="Fast_dds ON"$'\n'"${OPTIONAL_MODULES_TEXT}"
   echo ${OPTIONAL_MODULES_TEXT} > ${PWD}/Config/OptionalModules.ini
 
   if [ ! -f Makefile ]; then
@@ -200,7 +212,7 @@ fi
 if ${LAUNCH_UE4_EDITOR} ; then
 
   log "Launching UE4Editor..."
-  ${GDB} ${UE4_ROOT}/Engine/Binaries/Linux/UE4Editor "${PWD}/CarlaUE4.uproject" ${RHI} ${EDITOR_FLAGS}
+  ${GDB} ${UE4_ROOT}/Engine/Binaries/Linux/UE4Editor "${PWD}/CarlaUE4.uproject" ${RHI} ${EDITOR_FLAGS} ${EDITOR_ROS2_FLAGS}
 
 else
 
diff --git a/Util/BuildTools/BuildLibCarla.bat b/Util/BuildTools/BuildLibCarla.bat
index 710a5534a..b8aa6c583 100644
--- a/Util/BuildTools/BuildLibCarla.bat
+++ b/Util/BuildTools/BuildLibCarla.bat
@@ -15,11 +15,13 @@ rem -- Parse arguments ---------------------------------------------------------
 rem ============================================================================
 
 set DOC_STRING=Build LibCarla.
-set USAGE_STRING=Usage: %FILE_N% [-h^|--help] [--rebuild] [--server] [--client] [--clean]
+set USAGE_STRING=Usage: %FILE_N% [-h^|--help] [--rebuild] [--server] [--client] [--clean] [--ros2]
 
 set REMOVE_INTERMEDIATE=false
 set BUILD_SERVER=false
 set BUILD_CLIENT=false
+set M_ROS2=OFF
+set GENERATOR=""
 
 :arg-parse
 if not "%1"=="" (
@@ -34,6 +36,9 @@ if not "%1"=="" (
     if "%1"=="--client" (
         set BUILD_CLIENT=true
     )
+    if "%1"=="--ros2" (
+        set M_ROS2=ON
+    )
     if "%1"=="--clean" (
         set REMOVE_INTERMEDIATE=true
     )
@@ -73,7 +78,11 @@ rem
 set LIBCARLA_VSPROJECT_PATH=%INSTALLATION_DIR:/=\%libcarla-visualstudio\
 
 if %GENERATOR% == "" set GENERATOR="Visual Studio 16 2019"
-
+echo.%GENERATOR% | findstr /C:"Visual Studio" >nul && (
+    set PLATFORM=-A x64
+) || (
+    set PLATFORM=
+)
 
 set LIBCARLA_SERVER_INSTALL_PATH=%ROOT_PATH:/=\%Unreal\CarlaUE4\Plugins\Carla\CarlaDependencies\
 set LIBCARLA_CLIENT_INSTALL_PATH=%ROOT_PATH:/=\%PythonAPI\carla\dependencies\
@@ -104,12 +113,6 @@ if %REMOVE_INTERMEDIATE% == true (
 if not exist "%LIBCARLA_VSPROJECT_PATH%" mkdir "%LIBCARLA_VSPROJECT_PATH%"
 cd "%LIBCARLA_VSPROJECT_PATH%"
 
-echo.%GENERATOR% | findstr /C:"Visual Studio" >nul && (
-    set PLATFORM=-A x64
-) || (
-    set PLATFORM=
-)
-
 rem For some reason the findstr above sets an errorlevel even if it finds the string in this batch file.
 set errorlevel=0
 
@@ -121,6 +124,7 @@ if %BUILD_SERVER% == true (
       -DCMAKE_BUILD_TYPE=Server^
       -DCMAKE_CXX_FLAGS_RELEASE="/MD /MP"^
       -DCMAKE_INSTALL_PREFIX="%LIBCARLA_SERVER_INSTALL_PATH:\=/%"^
+      -DLIBCARLA_USE_ROS="%M_ROS2%"^
       "%ROOT_PATH%"
 
     if %errorlevel% neq 0 goto error_cmake
diff --git a/Util/BuildTools/BuildLibCarla.sh b/Util/BuildTools/BuildLibCarla.sh
index c9e2f13a0..149e65b9e 100755
--- a/Util/BuildTools/BuildLibCarla.sh
+++ b/Util/BuildTools/BuildLibCarla.sh
@@ -125,28 +125,31 @@ function build_libcarla {
 
   CMAKE_EXTRA_OPTIONS=''
 
+  M_ROS=false
   if [ $1 == Server ] ; then
-    M_TOOLCHAIN=${LIBCPP_TOOLCHAIN_FILE}
+    M_TOOLCHAIN=${CARLA_SERVER_TOOLCHAIN_FILE}
     M_BUILD_FOLDER=${LIBCARLA_BUILD_SERVER_FOLDER}.$(echo "$2" | tr '[:upper:]' '[:lower:]')
     M_INSTALL_FOLDER=${LIBCARLA_INSTALL_SERVER_FOLDER}
+  elif [ $1 == ServerROS ] ; then
+    BUILD_TYPE='Server'
+    M_TOOLCHAIN=${CARLA_SERVER_TOOLCHAIN_FILE}
+    M_BUILD_FOLDER=${LIBCARLA_BUILD_SERVER_FOLDER}.ROS.$(echo "$2" | tr '[:upper:]' '[:lower:]')
+    M_INSTALL_FOLDER=${LIBCARLA_INSTALL_SERVER_FOLDER}
+    M_ROS=true
   elif [ $1 == Client ] ; then
-    M_TOOLCHAIN=${LIBSTDCPP_TOOLCHAIN_FILE}
+    M_TOOLCHAIN=${CARLA_CLIENT_TOOLCHAIN_FILE}
     M_BUILD_FOLDER=${LIBCARLA_BUILD_CLIENT_FOLDER}.$(echo "$2" | tr '[:upper:]' '[:lower:]')
     M_INSTALL_FOLDER=${LIBCARLA_INSTALL_CLIENT_FOLDER}
   elif [ $1 == Pytorch ] ; then
-    M_TOOLCHAIN=${LIBSTDCPP_TOOLCHAIN_FILE}
+    M_TOOLCHAIN=${CARLA_SERVER_TOOLCHAIN_FILE}
     M_BUILD_FOLDER=${LIBCARLA_BUILD_PYTORCH_FOLDER}.$(echo "$2" | tr '[:upper:]' '[:lower:]')
     M_INSTALL_FOLDER=${LIBCARLA_INSTALL_SERVER_FOLDER}
-  elif [ $1 == ros2 ] ; then
-    M_TOOLCHAIN=${LIBSTDCPP_TOOLCHAIN_FILE}
-    M_BUILD_FOLDER=${LIBCARLA_FASTDDS_FOLDER}.$(echo "$2" | tr '[:upper:]' '[:lower:]')
-    M_INSTALL_FOLDER=${LIBCARLA_INSTALL_SERVER_FOLDER}
   elif [ $1 == ClientRSS ] ; then
     BUILD_TYPE='Client'
-    M_TOOLCHAIN=${LIBSTDCPP_TOOLCHAIN_FILE}
+    M_TOOLCHAIN=${CARLA_CLIENT_TOOLCHAIN_FILE}
     M_BUILD_FOLDER=${LIBCARLA_BUILD_CLIENT_FOLDER}.rss.$(echo "$2" | tr '[:upper:]' '[:lower:]')
     M_INSTALL_FOLDER=${LIBCARLA_INSTALL_CLIENT_FOLDER}
-    CMAKE_EXTRA_OPTIONS="${CMAKE_EXTRA_OPTIONS:+${CMAKE_EXTRA_OPTIONS} }-DBUILD_RSS_VARIANT=ON -DADRSS_INSTALL_DIR=${CARLA_BUILD_FOLDER}/ad-rss-4.4.4/install"
+    CMAKE_EXTRA_OPTIONS="${CMAKE_EXTRA_OPTIONS:+${CMAKE_EXTRA_OPTIONS} }-DBUILD_RSS_VARIANT=ON -DADRSS_INSTALL_DIR=${CARLA_BUILD_FOLDER}/ad-rss-4.4.5/install"
   else
     fatal_error "Invalid build configuration \"$1\""
   fi
@@ -184,6 +187,7 @@ function build_libcarla {
         -DCMAKE_BUILD_TYPE=${BUILD_TYPE:-$1} \
         -DLIBCARLA_BUILD_DEBUG=${M_DEBUG} \
         -DLIBCARLA_BUILD_RELEASE=${M_RELEASE} \
+        -DLIBCARLA_USE_ROS=${M_ROS} \
         -DCMAKE_TOOLCHAIN_FILE=${M_TOOLCHAIN} \
         -DCMAKE_INSTALL_PREFIX=${M_INSTALL_FOLDER} \
         -DCMAKE_EXPORT_COMPILE_COMMANDS=1 \
@@ -194,7 +198,7 @@ function build_libcarla {
 
   fi
 
-  ninja
+  ninja 
 
   ninja install | grep -v "Up-to-date:"
 
@@ -205,23 +209,22 @@ function build_libcarla {
 # -- Build all possible configurations -----------------------------------------
 # ==============================================================================
 
+SERVER_VARIANT='Server'
+if ${USE_ROS2}; then
+  SERVER_VARIANT='ServerROS'
+fi
 if { ${BUILD_SERVER} && ${BUILD_OPTION_DEBUG}; }; then
 
-  build_libcarla Server Debug
+  build_libcarla ${SERVER_VARIANT} Debug
 
 fi
 
 if { ${BUILD_SERVER} && ${BUILD_OPTION_RELEASE}; }; then
 
-  build_libcarla Server Release
+  build_libcarla ${SERVER_VARIANT} Release
   if ${USE_PYTORCH} ; then
     build_libcarla Pytorch Release
   fi
-
-  if ${USE_ROS2} ; then
-    build_libcarla ros2 Release
-  fi
-
 fi
 
 CLIENT_VARIANT='Client'
diff --git a/Util/BuildTools/BuildOSM2ODR.bat b/Util/BuildTools/BuildOSM2ODR.bat
index 77a4eff2b..c3ee9817c 100644
--- a/Util/BuildTools/BuildOSM2ODR.bat
+++ b/Util/BuildTools/BuildOSM2ODR.bat
@@ -6,6 +6,7 @@ rem Run it through a cmd with the x64 Visual C++ Toolset enabled.
 
 set LOCAL_PATH=%~dp0
 set FILE_N=-[%~n0]:
+set GENERATOR=""
 
 rem Print batch params (debug purpose)
 echo %FILE_N% [Batch params]: %*
@@ -57,6 +58,13 @@ if not "%1"=="" (
     goto :arg-parse
 )
 
+if %GENERATOR% == "" set GENERATOR="Visual Studio 16 2019"
+echo.%GENERATOR% | findstr /C:"Visual Studio" >nul && (
+    set PLATFORM=-A x64
+) || (
+    set PLATFORM=
+)
+
 if %REMOVE_INTERMEDIATE% == false (
     if %BUILD_OSM2ODR% == false (
         echo Nothing selected to be done.
@@ -76,8 +84,6 @@ set OSM2ODR_INSTALL_PATH=%ROOT_PATH:/=\%PythonAPI\carla\dependencies\
 set OSM2ODR__SERVER_INSTALL_PATH=%ROOT_PATH:/=\%Unreal\CarlaUE4\Plugins\Carla\CarlaDependencies
 set CARLA_DEPENDENCIES_FOLDER=%ROOT_PATH:/=\%Unreal\CarlaUE4\Plugins\Carla\CarlaDependencies\
 
-if %GENERATOR% == "" set GENERATOR="Visual Studio 16 2019"
-
 if %REMOVE_INTERMEDIATE% == true (
     rem Remove directories
     for %%G in (
@@ -90,6 +96,7 @@ if %REMOVE_INTERMEDIATE% == true (
     )
 )
 
+
 rem Build OSM2ODR
 if %BUILD_OSM2ODR% == true (
     cd "%INSTALLATION_DIR%"
@@ -99,7 +106,7 @@ if %BUILD_OSM2ODR% == true (
         del OSM2ODR.zip
         ren sumo-%CURRENT_OSM2ODR_COMMIT% osm2odr-source
     )
-    
+
     cd ..
     if not exist "%OSM2ODR_VSPROJECT_PATH%" mkdir "%OSM2ODR_VSPROJECT_PATH%"
     cd "%OSM2ODR_VSPROJECT_PATH%"
diff --git a/Util/BuildTools/BuildOSM2ODR.sh b/Util/BuildTools/BuildOSM2ODR.sh
index 6264c9a2f..d6ee51e6a 100755
--- a/Util/BuildTools/BuildOSM2ODR.sh
+++ b/Util/BuildTools/BuildOSM2ODR.sh
@@ -14,7 +14,7 @@ END
 REMOVE_INTERMEDIATE=false
 BUILD_OSM2ODR=false
 GIT_PULL=true
-CURRENT_OSM2ODR_COMMIT=1835e1e9538d0778971acc8b19b111834aae7261
+CURRENT_OSM2ODR_COMMIT=2a490962dc54da711ab09265393a4dc2f6d31813
 OSM2ODR_BRANCH=aaron/defaultsidewalkwidth
 OSM2ODR_REPO=https://github.com/carla-simulator/sumo.git
 
@@ -51,13 +51,15 @@ source $(dirname "$0")/Environment.sh
 
 function get_source_code_checksum {
   local EXCLUDE='*__pycache__*'
-  find "${OSM2ODR_SOURCE_FOLDER}"/* \! -path "${EXCLUDE}" -print0 | sha1sum | awk '{print $1}'
+  find "${OSM2ODR_BASENAME}-source"/* \! -path "${EXCLUDE}" -print0 | sha1sum | awk '{print $1}'
 }
 
 if ! { ${REMOVE_INTERMEDIATE} || ${BUILD_OSM2ODR}; }; then
   fatal_error "Nothing selected to be done."
 fi
 
+OSM2ODR_BASENAME=${CARLA_BUILD_FOLDER}/osm2odr
+
 # ==============================================================================
 # -- Clean intermediate files --------------------------------------------------
 # ==============================================================================
@@ -66,7 +68,8 @@ if ${REMOVE_INTERMEDIATE} ; then
 
   log "Cleaning intermediate files and folders."
 
-  rm -Rf ${OSM2ODR_BUILD_FOLDER}*
+  rm -Rf ${OSM2ODR_BASENAME}-client-build* ${OSM2ODR_BASENAME}-server-build*
+  rm -Rf ${OSM2ODR_BASENAME}-server-install* ${OSM2ODR_BASENAME}-client-install*
 
 fi
 
@@ -75,75 +78,64 @@ fi
 # ==============================================================================
 
 if ${BUILD_OSM2ODR} ; then
-  log "Building OSM2ODR."
-  if [ ! -d ${OSM2ODR_SOURCE_FOLDER} ] ; then
-    cd ${CARLA_BUILD_FOLDER}
-    curl --retry 5 --retry-max-time 120 -L -o OSM2ODR.zip https://github.com/carla-simulator/sumo/archive/${CURRENT_OSM2ODR_COMMIT}.zip
-    unzip -qq OSM2ODR.zip
-    rm -f OSM2ODR.zip
-    mv sumo-${CURRENT_OSM2ODR_COMMIT} ${OSM2ODR_SOURCE_FOLDER}
+
+  if [[ -d ${OSM2ODR_BASENAME}-client-install && -d ${OSM2ODR_BASENAME}-server-install ]] ; then
+    log "OSM2ODR already installed."
+  else
+    rm -Rf \
+        ${OSM2ODR_BASENAME}-source \
+        ${OSM2ODR_BASENAME}-server-build ${OSM2ODR_BASENAME}-client-build \
+        ${OSM2ODR_BASENAME}-server-install ${OSM2ODR_BASENAME}-client-install
+
+    log "Building OSM2ODR."
+    if [ ! -d ${OSM2ODR_BASENAME}-source ] ; then
+      cd ${CARLA_BUILD_FOLDER}
+      curl --retry 5 --retry-max-time 120 -L -o OSM2ODR.zip https://github.com/carla-simulator/sumo/archive/${CURRENT_OSM2ODR_COMMIT}.zip
+      unzip -qq OSM2ODR.zip
+      rm -f OSM2ODR.zip
+      mv sumo-${CURRENT_OSM2ODR_COMMIT} ${OSM2ODR_BASENAME}-source
+    fi
+
+    mkdir -p ${OSM2ODR_BASENAME}-client-build
+    pushd ${OSM2ODR_BASENAME}-client-build >/dev/null
+
+    cmake ${OSM2ODR_BASENAME}-source \
+        -G "Eclipse CDT4 - Ninja" \
+        -DCMAKE_INSTALL_PREFIX=${OSM2ODR_BASENAME}-client-install \
+        -DCMAKE_TOOLCHAIN_FILE="${CARLA_CLIENT_TOOLCHAIN_FILE}" \
+        -DOSM2ODR_INCLUDE_DIR=${CARLA_BUILD_FOLDER}/proj-client-install/include \
+        -DOSM2ODR_LIBRARY=${CARLA_BUILD_FOLDER}/proj-client-install/lib/libproj.so \
+        -DXercesC_INCLUDE_DIR=${CARLA_BUILD_FOLDER}/xerces-c-3.2.3-client-install/include \
+        -DXercesC_LIBRARY=${CARLA_BUILD_FOLDER}/xerces-c-3.2.3-client-install/lib/libxerces-c.so
+
+    ninja osm2odr
+    ninja install
+    popd >/dev/null
+
+    mkdir -p ${OSM2ODR_BASENAME}-server-build
+    pushd ${OSM2ODR_BASENAME}-server-build >/dev/null
+
+    cmake ${OSM2ODR_BASENAME}-source \
+        -G "Eclipse CDT4 - Ninja" \
+        -DCMAKE_INSTALL_PREFIX=${OSM2ODR_BASENAME}-server-install \
+        -DCMAKE_TOOLCHAIN_FILE="${CARLA_SERVER_TOOLCHAIN_FILE}" \
+        -DOSM2ODR_INCLUDE_DIR=${CARLA_BUILD_FOLDER}/proj-server-install/include \
+        -DOSM2ODR_LIBRARY=${CARLA_BUILD_FOLDER}/proj-server-install/lib/libproj.a \
+        -DXercesC_INCLUDE_DIR=${CARLA_BUILD_FOLDER}/xerces-c-3.2.3-server-install/include \
+        -DXercesC_LIBRARY=${CARLA_BUILD_FOLDER}/xerces-c-3.2.3-server-install/lib/libxerces-c.a
+
+    ninja osm2odr
+    ninja install
+    popd >/dev/null
+
+    rm -Rf ${OSM2ODR_BASENAME}-server-build ${OSM2ODR_BASENAME}-client-build
   fi
 
-  mkdir -p ${OSM2ODR_BUILD_FOLDER}
-  cd ${OSM2ODR_BUILD_FOLDER}
+  cp -p -r ${OSM2ODR_BASENAME}-server-install/include/* ${LIBCARLA_INSTALL_SERVER_FOLDER}/include/
+  cp -p ${OSM2ODR_BASENAME}-server-install/lib/*.a ${LIBCARLA_INSTALL_SERVER_FOLDER}/lib
 
-
-  export CC="$UE4_ROOT/Engine/Extras/ThirdPartyNotUE/SDKs/HostLinux/Linux_x64/v17_clang-10.0.1-centos7/x86_64-unknown-linux-gnu/bin/clang"
-  export CXX="$UE4_ROOT/Engine/Extras/ThirdPartyNotUE/SDKs/HostLinux/Linux_x64/v17_clang-10.0.1-centos7/x86_64-unknown-linux-gnu/bin/clang++"
-  export PATH="$UE4_ROOT/Engine/Extras/ThirdPartyNotUE/SDKs/HostLinux/Linux_x64/v17_clang-10.0.1-centos7/x86_64-unknown-linux-gnu/bin:$PATH"
-
-  cmake ${OSM2ODR_SOURCE_FOLDER} \
-      -G "Eclipse CDT4 - Ninja" \
-      -DCMAKE_CXX_FLAGS="-stdlib=libstdc++" \
-      -DCMAKE_INSTALL_PREFIX=${LIBCARLA_INSTALL_CLIENT_FOLDER} \
-      -DPROJ_INCLUDE_DIR=${CARLA_BUILD_FOLDER}/proj-install/include \
-      -DPROJ_LIBRARY=${CARLA_BUILD_FOLDER}/proj-install/lib/libproj.a \
-      -DXercesC_INCLUDE_DIR=${CARLA_BUILD_FOLDER}/xerces-c-3.2.3-install/include \
-      -DXercesC_LIBRARY=${CARLA_BUILD_FOLDER}/xerces-c-3.2.3-install/lib/libxerces-c.a
-
-  ninja osm2odr
-  ninja install
-
-  mkdir -p ${OSM2ODR_SERVER_BUILD_FOLDER}
-  cd ${OSM2ODR_SERVER_BUILD_FOLDER}
-
-  LLVM_BASENAME=llvm-8.0
-  LLVM_INCLUDE="$UE4_ROOT/Engine/Source/ThirdParty/Linux/LibCxx/include/c++/v1"
-  LLVM_LIBPATH="$UE4_ROOT/Engine/Source/ThirdParty/Linux/LibCxx/lib/Linux/x86_64-unknown-linux-gnu"
-
-  echo $LLVM_INCLUDE
-  echo $LLVM_LIBPATH
-
-  cmake ${OSM2ODR_SOURCE_FOLDER} \
-      -G "Eclipse CDT4 - Ninja" \
-      -DCMAKE_CXX_FLAGS="-fPIC -std=c++14 -stdlib=libc++ -I${LLVM_INCLUDE} -L${LLVM_LIBPATH}" \
-      -DCMAKE_INSTALL_PREFIX=${LIBCARLA_INSTALL_SERVER_FOLDER} \
-      -DPROJ_INCLUDE_DIR=${CARLA_BUILD_FOLDER}/proj-install-server/include \
-      -DPROJ_LIBRARY=${CARLA_BUILD_FOLDER}/proj-install-server/lib/libproj.a \
-      -DXercesC_INCLUDE_DIR=${CARLA_BUILD_FOLDER}/xerces-c-3.2.3-install-server/include \
-      -DXercesC_LIBRARY=${CARLA_BUILD_FOLDER}/xerces-c-3.2.3-install-server/lib/libxerces-c.a
-
-  ninja osm2odr
-  ninja install
-
-  mkdir -p ${OSM2ODR_SERVER_BUILD_FOLDER}
-  cd ${OSM2ODR_SERVER_BUILD_FOLDER}
-
-  LLVM_BASENAME=llvm-8.0
-  LLVM_INCLUDE="$UE4_ROOT/Engine/Source/ThirdParty/Linux/LibCxx/include/c++/v1"
-  LLVM_LIBPATH="$UE4_ROOT/Engine/Source/ThirdParty/Linux/LibCxx/lib/Linux/x86_64-unknown-linux-gnu"
-
-  cmake ${OSM2ODR_SOURCE_FOLDER} \
-      -G "Eclipse CDT4 - Ninja" \
-      -DCMAKE_CXX_FLAGS="-fPIC -std=c++14 -stdlib=libc++ -I${LLVM_INCLUDE} -L${LLVM_LIBPATH}" \
-      -DCMAKE_INSTALL_PREFIX=${LIBCARLA_INSTALL_SERVER_FOLDER} \
-      -DPROJ_INCLUDE_DIR=${CARLA_BUILD_FOLDER}/proj-install-server/include \
-      -DPROJ_LIBRARY=${CARLA_BUILD_FOLDER}/proj-install-server/lib/libproj.a \
-      -DXercesC_INCLUDE_DIR=${CARLA_BUILD_FOLDER}/xerces-c-3.2.3-install-server/include \
-      -DXercesC_LIBRARY=${CARLA_BUILD_FOLDER}/xerces-c-3.2.3-install-server/lib/libxerces-c.a
-
-  ninja osm2odr
-  ninja install
+  cp -p -r ${OSM2ODR_BASENAME}-client-install/include/* ${LIBCARLA_INSTALL_CLIENT_FOLDER}/include/
+  cp -p ${OSM2ODR_BASENAME}-client-install/lib/*.a ${LIBCARLA_INSTALL_CLIENT_FOLDER}/lib
 
 fi
 
diff --git a/Util/BuildTools/BuildOSMRenderer.bat b/Util/BuildTools/BuildOSMRenderer.bat
index fbf8367e1..ca4e46045 100644
--- a/Util/BuildTools/BuildOSMRenderer.bat
+++ b/Util/BuildTools/BuildOSMRenderer.bat
@@ -75,7 +75,7 @@ if not exist "%OSM_RENDERER_VSPROJECT_PATH%" mkdir "%OSM_RENDERER_VSPROJECT_PATH
 cd "%OSM_RENDERER_VSPROJECT_PATH%"
 
 cmake -G "Visual Studio 16 2019" -A x64^
-    -DCMAKE_CXX_FLAGS_RELEASE="/std:c++17 /wd4251 /I%INSTALLATION_DIR:/=\%boost-1.80.0-install\include"^
+    -DCMAKE_CXX_FLAGS_RELEASE="/std:c++20 /wd4251 /I%INSTALLATION_DIR:/=\%boost-1.80.0-install\include"^
     "%OSM_RENDERER_SOURCE%"
 
 cmake --build . --config Release
diff --git a/Util/BuildTools/BuildOSMRenderer.sh b/Util/BuildTools/BuildOSMRenderer.sh
index 0cb2287eb..1fbaf7ef6 100755
--- a/Util/BuildTools/BuildOSMRenderer.sh
+++ b/Util/BuildTools/BuildOSMRenderer.sh
@@ -68,7 +68,7 @@ echo "Building osm-map-renderer"
 mkdir -p ${OSM_RENDERER_BUILD}
 cd ${OSM_RENDERER_BUILD}
 
-cmake -DCMAKE_CXX_FLAGS="-std=c++17 -g -pthread -I${CARLA_BUILD_FOLDER}/boost-1.80.0-c10-install/include" \
+cmake -DCMAKE_TOOLCHAIN_FILE="${CARLA_CLIENT_TOOLCHAIN_FILE}" -DCMAKE_CXX_FLAGS="-g -I${CARLA_BUILD_FOLDER}/boost-1.80.0-install/include" \
     ${OSM_RENDERER_SOURCE}
 make
 
diff --git a/Util/BuildTools/BuildPythonAPI.sh b/Util/BuildTools/BuildPythonAPI.sh
index f5bbb5c61..a487f20d2 100755
--- a/Util/BuildTools/BuildPythonAPI.sh
+++ b/Util/BuildTools/BuildPythonAPI.sh
@@ -48,10 +48,10 @@ while [[ $# -gt 0 ]]; do
   esac
 done
 
-
-export CC="$UE4_ROOT/Engine/Extras/ThirdPartyNotUE/SDKs/HostLinux/Linux_x64/v17_clang-10.0.1-centos7/x86_64-unknown-linux-gnu/bin/clang"
-export CXX="$UE4_ROOT/Engine/Extras/ThirdPartyNotUE/SDKs/HostLinux/Linux_x64/v17_clang-10.0.1-centos7/x86_64-unknown-linux-gnu/bin/clang++"
-export PATH="$UE4_ROOT/Engine/Extras/ThirdPartyNotUE/SDKs/HostLinux/Linux_x64/v17_clang-10.0.1-centos7/x86_64-unknown-linux-gnu/bin:$PATH"
+UNREAL_SYSROOT="$UE4_ROOT/Engine/Extras/ThirdPartyNotUE/SDKs/HostLinux/Linux_x64/v17_clang-10.0.1-centos7/x86_64-unknown-linux-gnu"
+export CC="${UNREAL_SYSROOT}/bin/clang"
+export CXX="${UNREAL_SYSROOT}/bin/clang++"
+export PATH="${UNREAL_SYSROOT}/bin:$PATH"
 
 source $(dirname "$0")/Environment.sh
 
@@ -75,7 +75,7 @@ if ${REMOVE_INTERMEDIATE} ; then
   rm -Rf build dist source/carla.egg-info
 
   find source -name "*.so" -delete
-  find source -name "__pycache__" -type d -exec rm -r "{}" \;
+  find source -name "__pycache__" -type d -exec rm -rf "{}" \;
 
 fi
 
diff --git a/Util/BuildTools/BuildUtilsDocker.sh b/Util/BuildTools/BuildUtilsDocker.sh
index 2c85a7b05..ced4f4e9c 100755
--- a/Util/BuildTools/BuildUtilsDocker.sh
+++ b/Util/BuildTools/BuildUtilsDocker.sh
@@ -37,7 +37,7 @@ mkdir -p "${FBX2OBJ_BUILD_FOLDER}"
 pushd "${FBX2OBJ_BUILD_FOLDER}" >/dev/null
 
 cmake -G "Ninja" \
-    -DCMAKE_CXX_FLAGS="-fPIC -std=c++14" \
+    -DCMAKE_CXX_FLAGS="-fPIC -std=c++20" \
     ..
 
 # copy the shared object 'libfbxsdk.so' to 'dist' folder
diff --git a/Util/BuildTools/Linux.mk b/Util/BuildTools/Linux.mk
index 8e64f7689..286e6293f 100644
--- a/Util/BuildTools/Linux.mk
+++ b/Util/BuildTools/Linux.mk
@@ -7,6 +7,10 @@ launch: LibCarla.server.release osm2odr downloadplugins
 	@${CARLA_BUILD_TOOLS_FOLDER}/BuildUE4Plugins.sh --build $(ARGS)
 	@${CARLA_BUILD_TOOLS_FOLDER}/BuildCarlaUE4.sh --build --launch $(ARGS)
 
+launch.debug: LibCarla.server.debug osm2odr downloadplugins
+	@${CARLA_BUILD_TOOLS_FOLDER}/BuildUE4Plugins.sh --build $(ARGS)
+	@${CARLA_BUILD_TOOLS_FOLDER}/BuildCarlaUE4.sh --build --debug --launch $(ARGS)
+
 launch-only:
 	@${CARLA_BUILD_TOOLS_FOLDER}/BuildCarlaUE4.sh --launch $(ARGS)
 
@@ -138,7 +142,7 @@ LibCarla.client.rss.release: setup ad-rss
 plugins:
 	@${CARLA_BUILD_TOOLS_FOLDER}/Plugins.sh $(ARGS)
 
-setup downloadplugins:
+setup: downloadplugins
 	@${CARLA_BUILD_TOOLS_FOLDER}/Setup.sh $(ARGS)
 
 ad-rss:
diff --git a/Util/BuildTools/Package.sh b/Util/BuildTools/Package.sh
index 0be72293a..199740794 100755
--- a/Util/BuildTools/Package.sh
+++ b/Util/BuildTools/Package.sh
@@ -6,7 +6,7 @@
 
 DOC_STRING="Makes a packaged version of CARLA and other content packages ready for distribution."
 
-USAGE_STRING="Usage: $0 [-h|--help] [--config={Debug,Development,Shipping}] [--no-zip] [--clean-intermediate] [--packages=Name1,Name2,...] [--target-archive=] [--archive-sufix=]"
+USAGE_STRING="Usage: $0 [-h|--help] [--config={Debug,Development,Shipping}] [--no-zip] [--clean-intermediate] [--packages=Name1,Name2,...] [--target-archive=] [--archive-sufix=] [--ros2]"
 
 PACKAGES="Carla"
 DO_TARBALL=true
@@ -17,7 +17,10 @@ USE_CARSIM=false
 SINGLE_PACKAGE=false
 ARCHIVE_SUFIX=""
 
-OPTS=`getopt -o h --long help,config:,no-zip,clean-intermediate,carsim,packages:,python-version,target-archive:,archive-sufix:, -n 'parse-options' -- "$@"`
+EDITOR_ROS2_FLAGS=""
+
+
+OPTS=`getopt -o h --long help,config:,no-zip,clean-intermediate,carsim,packages:,python-version,target-archive:,archive-sufix:,ros2 -n 'parse-options' -- "$@"`
 
 eval set -- "$OPTS"
 
@@ -42,6 +45,11 @@ while [[ $# -gt 0 ]]; do
     --archive-sufix )
       ARCHIVE_SUFIX="$2"
       shift 2 ;;
+    --ros2 )
+      # Due to continued segfaults in reallocations of MallocBinned2 enforce using AnsiMalloc calls 
+      # (see https://forums.unrealengine.com/t/dealing-with-allocator-mismatches-with-external-libraries/1416830)
+      EDITOR_ROS2_FLAGS="-ansimalloc"
+      shift ;;
     --carsim )
       USE_CARSIM=true;
       shift ;;
@@ -182,12 +190,14 @@ if ${DO_CARLA_RELEASE} ; then
   fi
 
   if [ -d "./Unreal/CarlaUE4/Plugins/Carla/CarlaDependencies/lib" ] ; then
-    cp -r "./Unreal/CarlaUE4/Plugins/Carla/CarlaDependencies/lib" "${DESTINATION}/CarlaUE4/Plugins/Carla/CarlaDependencies"
+    cp -r "./Unreal/CarlaUE4/Plugins/Carla/CarlaDependencies/lib/" "${DESTINATION}/CarlaUE4/Plugins/Carla/CarlaDependencies"
   fi
 
   copy_if_changed "./Unreal/CarlaUE4/Content/Carla/HDMaps/*.pcd" "${DESTINATION}/HDMaps/"
   copy_if_changed "./Unreal/CarlaUE4/Content/Carla/HDMaps/Readme.md" "${DESTINATION}/HDMaps/README"
 
+  sed -i "s/CarlaUE4 /CarlaUE4 ${EDITOR_ROS2_FLAGS} /g" "${DESTINATION}/CarlaUE4.sh"
+
   popd >/dev/null
 
 fi
diff --git a/Util/BuildTools/Setup.bat b/Util/BuildTools/Setup.bat
index 5a496a966..5aa9be672 100644
--- a/Util/BuildTools/Setup.bat
+++ b/Util/BuildTools/Setup.bat
@@ -65,7 +65,7 @@ if not "%1"=="" (
 
 rem If not defined, use Visual Studio 2019 as tool set
 if "%TOOLSET%" == "" set TOOLSET=msvc-14.2
-if %GENERATOR% == "" set GENERATOR="Visual Studio 16 2019"
+if "%GENERATOR%" == "" set GENERATOR="Visual Studio 16 2019"
 
 rem If is not set, set the number of parallel jobs to the number of CPU threads
 if "%NUMBER_OF_ASYNC_JOBS%" == "" set NUMBER_OF_ASYNC_JOBS=%NUMBER_OF_PROCESSORS%
@@ -81,14 +81,17 @@ echo %FILE_N% Boost toolset:      %TOOLSET%
 echo %FILE_N% Generator:          %GENERATOR%
 echo %FILE_N% Install directory:  "%INSTALLATION_DIR%"
 
-if not exist "%CONTENT_DIR%" (
-    echo %FILE_N% Creating "%CONTENT_DIR%" folder...
-    mkdir "%CONTENT_DIR%"
-)
-
-if not exist "%INSTALLATION_DIR%" (
-    echo %FILE_N% Creating "%INSTALLATION_DIR%" folder...
-    mkdir "%INSTALLATION_DIR%"
+rem creating some folders
+for %%G in (
+    "%CONTENT_DIR%",
+    "%INSTALLATION_DIR%",
+    "%CARLA_DEPENDENCIES_FOLDER%\include",
+    "%CARLA_DEPENDENCIES_FOLDER%\lib"
+) do (
+    if not exist %%G (
+        echo %FILE_N% Creating "%%G" folder...
+        mkdir "%%G"
+    )
 )
 
 rem ============================================================================
@@ -141,6 +144,8 @@ if not defined install_rpclib (
     echo %FILE_N% Failed while installing rpclib.
     goto failed
 )
+xcopy /Y /S /I %INSTALLATION_DIR%rpclib-install\include\rpc\* %CARLA_DEPENDENCIES_FOLDER%\include\rpc\* > NUL
+copy %INSTALLATION_DIR%rpclib-install\lib\*.* %CARLA_DEPENDENCIES_FOLDER%\lib\ > NUL
 
 rem ============================================================================
 rem -- Download and install Google Test ----------------------------------------
@@ -178,26 +183,6 @@ if not defined install_recast (
     set RECAST_INSTALL_DIR=%install_recast:\=/%
 )
 
-rem ============================================================================
-rem -- Download and install Fast-DDS (for ROS2)---------------------------------
-rem ============================================================================
-
-if %USE_ROS2% == true (
-    echo %FILE_N% Installing "Fast-DDS"...
-    call "%INSTALLERS_DIR%install_fastDDS.bat"^
-    --build-dir "%INSTALLATION_DIR%"
-
-    if %errorlevel% neq 0 goto failed
-
-    if not defined install_dds (
-
-        echo %FILE_N% Failed while installing "Fast-DDS".
-        goto failed
-    ) else (
-        set FASTDDS_INSTALL_DIR=%install_dds:\=/%
-    )
-)
-
 rem ============================================================================
 rem -- Download and install Boost ----------------------------------------------
 rem ============================================================================
@@ -216,6 +201,27 @@ if not defined install_boost (
     goto failed
 )
 
+rem ============================================================================
+rem -- Download and install Fast-DDS (for ROS2)---------------------------------
+rem ============================================================================
+
+if %USE_ROS2% == true (
+    echo %FILE_N% Installing "Fast-DDS"...
+    call "%INSTALLERS_DIR%install_fastDDS.bat"^
+    --boost-version %BOOST_VERSION%^
+    --build-dir "%INSTALLATION_DIR%"^
+    --install-dir "%INSTALLATION_DIR%fastDDS-install\"
+
+    if %errorlevel% neq 0 goto failed
+
+    if not defined install_dds (
+        echo %FILE_N% Failed while installing "Fast-DDS".
+        goto failed
+    )
+    copy %INSTALLATION_DIR%fastDDS-install\lib\*.lib %CARLA_DEPENDENCIES_FOLDER%\lib > NUL
+    xcopy /Y /S /I  %INSTALLATION_DIR%fastDDS-install\include\* %CARLA_DEPENDENCIES_FOLDER%\include\* > NUL
+)
+
 rem ============================================================================
 rem -- Download and install Xercesc --------------------------------------------
 rem ============================================================================
@@ -285,81 +291,6 @@ if %USE_CHRONO% == true (
     xcopy /Y /S /I "%INSTALLATION_DIR%eigen-install\include\*" "%CARLA_DEPENDENCIES_FOLDER%include\*" > NUL
 )
 
-REM ==============================================================================
-REM -- Download Fast DDS and dependencies ----------------------------------------
-REM ==============================================================================
-
-SET FASTDDS_BASENAME=fast-dds
-SET FASTDDS_INSTALL_DIR=%CD%\%FASTDDS_BASENAME%-install
-SET FASTDDS_INCLUDE=%FASTDDS_INSTALL_DIR%\include
-SET FASTDDS_LIB=%FASTDDS_INSTALL_DIR%\lib
-IF "%USE_ROS2%"=="true" (
-
-  :build_fastdds_extension
-  SET LIB_SOURCE=%1
-  SET LIB_REPO=%2
-  SET CMAKE_FLAGS=%3
-
-  IF NOT EXIST "%LIB_SOURCE%" (
-    mkdir "%LIB_SOURCE%"
-    echo %LIB_REPO%
-    git clone %LIB_REPO% %LIB_SOURCE%
-    mkdir "%LIB_SOURCE%\build"
-  )
-
-  IF NOT EXIST "%FASTDDS_INSTALL_DIR%" (
-    mkdir "%FASTDDS_INSTALL_DIR%"
-    echo Build foonathan memory vendor
-    SET FOONATHAN_MEMORY_VENDOR_BASENAME=foonathan-memory-vendor
-    SET FOONATHAN_MEMORY_VENDOR_SOURCE_DIR=%CD%\%FOONATHAN_MEMORY_VENDOR_BASENAME%-source
-    SET FOONATHAN_MEMORY_VENDOR_REPO="https://github.com/eProsima/foonathan_memory_vendor.git"
-    SET FOONATHAN_MEMORY_VENDOR_CMAKE_FLAGS=-DBUILD_SHARED_LIBS=ON
-    CALL :build_fastdds_extension "%FOONATHAN_MEMORY_VENDOR_SOURCE_DIR%" "%FOONATHAN_MEMORY_VENDOR_REPO%"
-    pushd "%FOONATHAN_MEMORY_VENDOR_SOURCE_DIR%\build" >nul
-    cmake -G "Ninja" ^
-      -DCMAKE_INSTALL_PREFIX="%FASTDDS_INSTALL_DIR%" ^
-      -DBUILD_SHARED_LIBS=ON ^
-      -DCMAKE_CXX_FLAGS_RELEASE="-D_GLIBCXX_USE_CXX11_ABI=0" ^
-      -DFOONATHAN_MEMORY_FORCE_VENDORED_BUILD=ON ^
-      ..
-    ninja
-    ninja install
-    popd >nul
-    rmdir /s /q "%FOONATHAN_MEMORY_VENDOR_SOURCE_DIR%"
-
-    echo Build fast cdr
-    SET FAST_CDR_BASENAME=fast-cdr
-    SET FAST_CDR_SOURCE_DIR=%CD%\%FAST_CDR_BASENAME%-source
-    SET FAST_CDR_REPO="https://github.com/eProsima/Fast-CDR.git"
-    CALL :build_fastdds_extension "%FAST_CDR_SOURCE_DIR%" "%FAST_CDR_REPO%"
-    pushd "%FAST_CDR_SOURCE_DIR%\build" >nul
-    cmake -G "Ninja" ^
-      -DCMAKE_INSTALL_PREFIX="%FASTDDS_INSTALL_DIR%" ^
-      -DCMAKE_CXX_FLAGS_RELEASE="-D_GLIBCXX_USE_CXX11_ABI=0" ^
-      ..
-    ninja
-    ninja install
-    popd >nul
-    rmdir /s /q "%FAST_CDR_SOURCE_DIR%"
-
-    echo Build fast dds
-    SET FAST_DDS_LIB_BASENAME=fast-dds-lib
-    SET FAST_DDS_LIB_SOURCE_DIR=%CD%\%FAST_DDS_LIB_BASENAME%-source
-    SET FAST_DDS_LIB_REPO="https://github.com/eProsima/Fast-DDS.git"
-    CALL :build_fastdds_extension "%FAST_DDS_LIB_SOURCE_DIR%" "%FAST_DDS_LIB_REPO%"
-    pushd "%FAST_DDS_LIB_SOURCE_DIR%\build" >nul
-    cmake -G "Ninja" ^
-      -DCMAKE_INSTALL_PREFIX="%FASTDDS_INSTALL_DIR%" ^
-      -DCMAKE_CXX_FLAGS=-latomic ^
-      -DCMAKE_CXX_FLAGS_RELEASE="-D_GLIBCXX_USE_CXX11_ABI=0" ^
-      ..
-    ninja
-    ninja install
-    popd >nul
-    rmdir /
-  )
-)
-
 rem ============================================================================
 rem -- Assets download URL -----------------------------------------------------
 rem ============================================================================
@@ -384,7 +315,7 @@ set CMAKE_CONFIG_FILE=%INSTALLATION_DIR%CMakeLists.txt.in
  >"%CMAKE_CONFIG_FILE%" echo # Automatically generated by Setup.bat
 >>"%CMAKE_CONFIG_FILE%" echo set(CARLA_VERSION %carla_version%)
 >>"%CMAKE_CONFIG_FILE%" echo.
->>"%CMAKE_CONFIG_FILE%" echo set(CMAKE_CXX_STANDARD 14)
+>>"%CMAKE_CONFIG_FILE%" echo set(CMAKE_CXX_STANDARD 20)
 >>"%CMAKE_CONFIG_FILE%" echo set(CMAKE_CXX_STANDARD_REQUIRED ON)
 >>"%CMAKE_CONFIG_FILE%" echo.
 >>"%CMAKE_CONFIG_FILE%" echo add_definitions(-D_WIN32_WINNT=0x0600)
@@ -400,6 +331,11 @@ set CMAKE_CONFIG_FILE=%INSTALLATION_DIR%CMakeLists.txt.in
 >>"%CMAKE_CONFIG_FILE%" echo set(RPCLIB_INCLUDE_PATH "%CMAKE_INSTALLATION_DIR%rpclib-install/include")
 >>"%CMAKE_CONFIG_FILE%" echo set(RPCLIB_LIB_PATH "%CMAKE_INSTALLATION_DIR%rpclib-install/lib")
 >>"%CMAKE_CONFIG_FILE%" echo.
+>>"%CMAKE_CONFIG_FILE%" echo set(ROS2_MW_INCLUDE_PATH "%CMAKE_INSTALLATION_DIR%fastDDS-install/include")
+>>"%CMAKE_CONFIG_FILE%" echo set(ROS2_MW_LIB_PATH "%CMAKE_INSTALLATION_DIR%fastDDS-install/lib")
+>>"%CMAKE_CONFIG_FILE%" echo set(ROS2_MW_LINK_LIBRARIES "fastrtps;fastcdr")
+>>"%CMAKE_CONFIG_FILE%" echo set(ROS2_MW_NAME "fastdds")
+>>"%CMAKE_CONFIG_FILE%" echo.
 >>"%CMAKE_CONFIG_FILE%" echo if (CMAKE_BUILD_TYPE STREQUAL "Server")
 >>"%CMAKE_CONFIG_FILE%" echo   # Prevent exceptions
 >>"%CMAKE_CONFIG_FILE%" echo   add_definitions(-DBOOST_TYPE_INDEX_FORCE_NO_RTTI_COMPATIBILITY)
diff --git a/Util/BuildTools/Setup.sh b/Util/BuildTools/Setup.sh
index 64774d017..4d0a91616 100755
--- a/Util/BuildTools/Setup.sh
+++ b/Util/BuildTools/Setup.sh
@@ -47,47 +47,133 @@ done
 
 source $(dirname "$0")/Environment.sh
 
-export CC="$UE4_ROOT/Engine/Extras/ThirdPartyNotUE/SDKs/HostLinux/Linux_x64/v17_clang-10.0.1-centos7/x86_64-unknown-linux-gnu/bin/clang"
-export CXX="$UE4_ROOT/Engine/Extras/ThirdPartyNotUE/SDKs/HostLinux/Linux_x64/v17_clang-10.0.1-centos7/x86_64-unknown-linux-gnu/bin/clang++"
-export PATH="$UE4_ROOT/Engine/Extras/ThirdPartyNotUE/SDKs/HostLinux/Linux_x64/v17_clang-10.0.1-centos7/x86_64-unknown-linux-gnu/bin:$PATH"
-
-CXX_TAG=c10
-
 # Convert comma-separated string to array of unique elements.
 IFS="," read -r -a PY_VERSION_LIST <<< "${PY_VERSION_LIST}"
 
 mkdir -p ${CARLA_BUILD_FOLDER}
 pushd ${CARLA_BUILD_FOLDER} >/dev/null
 
-LLVM_INCLUDE="$UE4_ROOT/Engine/Source/ThirdParty/Linux/LibCxx/include/c++/v1"
-LLVM_LIBPATH="$UE4_ROOT/Engine/Source/ThirdParty/Linux/LibCxx/lib/Linux/x86_64-unknown-linux-gnu"
-UNREAL_HOSTED_CFLAGS="--sysroot=$UE4_ROOT/Engine/Extras/ThirdPartyNotUE/SDKs/HostLinux/Linux_x64/v17_clang-10.0.1-centos7/x86_64-unknown-linux-gnu/"
+LLVM_INCLUDE="${UE4_ROOT}/Engine/Source/ThirdParty/Linux/LibCxx/include/c++/v1"
+LLVM_LIBPATH="${UE4_ROOT}/Engine/Source/ThirdParty/Linux/LibCxx/lib/Linux/x86_64-unknown-linux-gnu"
+UNREAL_SYSROOT="$UE4_ROOT/Engine/Extras/ThirdPartyNotUE/SDKs/HostLinux/Linux_x64/v17_clang-10.0.1-centos7/x86_64-unknown-linux-gnu"
+UNREAL_HOSTED_CFLAGS="--sysroot=${UNREAL_SYSROOT} -isystem ${LLVM_INCLUDE}"
+UNREAL_SYSTEM_LIBPATH="${UNREAL_SYSROOT}/usr/lib"
+UNREAL_HOSTED_LINKER_FLAGS="--sysroot=${UNREAL_SYSROOT} -L${LLVM_LIBPATH} -L${UNREAL_SYSTEM_LIBPATH} -lc++ -lc++abi"
+
+
+CARLA_CFLAGS="-fPIC -w"
+CARLA_CXXFLAGS="-std=c++20 -DBOOST_ERROR_CODE_HEADER_ONLY"
+
+CARLA_SERVER_CFLAGS="${UNREAL_HOSTED_CFLAGS} ${CARLA_CFLAGS}"
+CARLA_SERVER_CXXFLAGS="${CARLA_SERVER_CFLAGS} ${CARLA_CXXFLAGS} -stdlib=libc++ -DBOOST_NO_EXCEPTIONS -DASIO_NO_EXCEPTIONS"
+CARLA_SERVER_LINKER_FLAGS="${UNREAL_HOSTED_LINKER_FLAGS} -L${LIBCARLA_INSTALL_SERVER_FOLDER}/lib/"
+
+export CC="${UNREAL_SYSROOT}/bin/clang"
+export CXX="${UNREAL_SYSROOT}/bin/clang++"
+export PATH="${UNREAL_SYSROOT}/bin:$PATH"
+
+# ==============================================================================
+# -- Generate Version.h --------------------------------------------------------
+# ==============================================================================
+
+CARLA_VERSION=$(get_git_repository_version)
+
+log "CARLA version ${CARLA_VERSION}."
+
+VERSION_H_FILE=${LIBCARLA_ROOT_FOLDER}/source/carla/Version.h
+VERSION_H_FILE_GEN=${CARLA_BUILD_FOLDER}/Version.h
+
+sed -e "s|\${CARLA_VERSION}|${CARLA_VERSION}|g" ${VERSION_H_FILE}.in > ${VERSION_H_FILE_GEN}
+
+move_if_changed "${VERSION_H_FILE_GEN}" "${VERSION_H_FILE}"
+
+# ==============================================================================
+# -- Generate CMake toolchains and config --------------------------------------
+# ==============================================================================
+
+log "Generating CMake configuration files."
+
+# -- CARLA_CLIENT_TOOLCHAIN_FILE --------------------------------------------------
+
+cat >${CARLA_CLIENT_TOOLCHAIN_FILE}.gen <<EOL
+# Automatically generated by `basename "$0"`
+
+set(CMAKE_C_COMPILER ${CC})
+set(CMAKE_CXX_COMPILER ${CXX})
+
+# disable -Werror since the boost 1.72 doesn't compile with ad_rss without warnings (i.e. the geometry headers)
+set(CMAKE_C_FLAGS "\${CMAKE_C_FLAGS} ${CARLA_CFLAGS}" CACHE STRING "" FORCE)
+set(CMAKE_CXX_FLAGS "\${CMAKE_CXX_FLAGS} ${CARLA_CFLAGS} ${CARLA_CXXFLAGS} -pthread" CACHE STRING "" FORCE)
+set(CMAKE_CXX_FLAGS "\${CMAKE_CXX_FLAGS} -Wall -Wextra -Wpedantic" CACHE STRING "" FORCE)
+set(CMAKE_CXX_FLAGS "\${CMAKE_CXX_FLAGS} -Wdeprecated -Wshadow -Wuninitialized -Wunreachable-code" CACHE STRING "" FORCE)
+set(CMAKE_CXX_FLAGS "\${CMAKE_CXX_FLAGS} -Wpessimizing-move -Wold-style-cast -Wnull-dereference" CACHE STRING "" FORCE)
+set(CMAKE_CXX_FLAGS "\${CMAKE_CXX_FLAGS} -Wduplicate-enum -Wnon-virtual-dtor -Wheader-hygiene" CACHE STRING "" FORCE)
+set(CMAKE_CXX_FLAGS "\${CMAKE_CXX_FLAGS} -Wconversion -Wfloat-overflow-conversion" CACHE STRING "" FORCE)
+
+# @todo These flags need to be compatible with setup.py compilation.
+set(CMAKE_CXX_FLAGS_RELEASE_CLIENT "\${CMAKE_CXX_FLAGS_RELEASE} -DNDEBUG -g -fwrapv -O2 -Wall -Wstrict-prototypes -fno-strict-aliasing -Wdate-time -D_FORTIFY_SOURCE=2 -g -fstack-protector-strong -Wformat -Werror=format-security ${CARLA_CFLAGS} ${CARLA_CXXFLAGS} -Wno-missing-braces" CACHE STRING "" FORCE)
+EOL
+
+# -- CARLA_SERVER_TOOLCHAIN_FILE -----------------------------------------------------
+
+# We can reuse the previous toolchain.
+cp ${CARLA_CLIENT_TOOLCHAIN_FILE}.gen ${CARLA_SERVER_TOOLCHAIN_FILE}.gen
+
+cat >>${CARLA_SERVER_TOOLCHAIN_FILE}.gen <<EOL
+
+set(BUILD_STATIC_LIBS ON)
+set(BUILD_SHARED_LIBS OFF)
+set(CMAKE_CXX_FLAGS "\${CMAKE_CXX_FLAGS} ${CARLA_SERVER_CXXFLAGS}" CACHE STRING "" FORCE)
+set(CMAKE_SHARED_LINKER_FLAGS "\${CMAKE_SHARED_LINKER_FLAGS} ${CARLA_SERVER_LINKER_FLAGS}" CACHE STRING "" FORCE)
+set(CMAKE_EXE_LINKER_FLAGS "\${CMAKE_EXE_LINKER_FLAGS} ${CARLA_SERVER_LINKER_FLAGS}" CACHE STRING "" FORCE)
+set(CMAKE_INSTALL_RPATH "\$ORIGIN/../../CarlaDependencies/lib")
+set(CMAKE_BUILD_RPATH_USE_ORIGIN True)
+
+EOL
+
+move_if_changed "${CARLA_SERVER_TOOLCHAIN_FILE}.gen" "${CARLA_SERVER_TOOLCHAIN_FILE}"
+move_if_changed "${CARLA_CLIENT_TOOLCHAIN_FILE}.gen" "${CARLA_CLIENT_TOOLCHAIN_FILE}"
+
+
+mkdir -p "${LIBCARLA_INSTALL_SERVER_FOLDER}/lib/"
+mkdir -p "${LIBCARLA_INSTALL_SERVER_FOLDER}/include/"
+mkdir -p "${LIBCARLA_INSTALL_CLIENT_FOLDER}/include/system"
+mkdir -p "${LIBCARLA_INSTALL_CLIENT_FOLDER}/lib"
+mkdir -p "${LIBCARLA_INSTALL_CLIENT_FOLDER}/bin/"
+
+# Install libc++ libraries.
+cp ${LLVM_LIBPATH}/libc++*.a "${LIBCARLA_INSTALL_SERVER_FOLDER}/lib/"
+
+# Install UE shared dependencies
+cp -p ${UNREAL_SYSTEM_LIBPATH}/lib${libName}*.so* "${LIBCARLA_INSTALL_SERVER_FOLDER}/lib/"
 
 # ==============================================================================
 # -- Get boost includes --------------------------------------------------------
 # ==============================================================================
 
 BOOST_VERSION=1.80.0
-BOOST_BASENAME="boost-${BOOST_VERSION}-${CXX_TAG}"
+BOOST_BASENAME="boost-${BOOST_VERSION}"
 BOOST_SHA256SUM="4b2136f98bdd1f5857f1c3dea9ac2018effe65286cf251534b6ae20cc45e1847"
 
-BOOST_INCLUDE=${PWD}/${BOOST_BASENAME}-install/include
-BOOST_LIBPATH=${PWD}/${BOOST_BASENAME}-install/lib
+BOOST_CLIENT_INCLUDE=${PWD}/${BOOST_BASENAME}-client-install/include
+BOOST_CLIENT_LIBPATH=${PWD}/${BOOST_BASENAME}-client-install/lib
+BOOST_SERVER_INCLUDE=${PWD}/${BOOST_BASENAME}-server-install/include
+BOOST_SERVER_LIBPATH=${PWD}/${BOOST_BASENAME}-server-install/lib
 
 for PY_VERSION in ${PY_VERSION_LIST[@]} ; do
   SHOULD_BUILD_BOOST=true
   PYTHON_VERSION=$(/usr/bin/env python${PY_VERSION} -V 2>&1)
   LIB_NAME=$(cut -d . -f 1,2 <<< "$PYTHON_VERSION" | tr -d .)
   LIB_NAME=${LIB_NAME:7}
-  if [[ -d "${BOOST_BASENAME}-install" ]] ; then
-    if [ -f "${BOOST_BASENAME}-install/lib/libboost_python${LIB_NAME}.a" ] ; then
+  if [[ -d "${BOOST_BASENAME}-server-install" && -d "${BOOST_BASENAME}-client-install" ]] ; then
+    if [ -f "${BOOST_BASENAME}-server-install/lib/libboost_system.a" ] && [ -f "${BOOST_BASENAME}-client-install/lib/libboost_python${LIB_NAME}.a" ] ; then
       SHOULD_BUILD_BOOST=false
       log "${BOOST_BASENAME} already installed."
     fi
   fi
 
   if { ${SHOULD_BUILD_BOOST} ; } ; then
-    rm -Rf ${BOOST_BASENAME}-source
+    rm -Rf ${BOOST_BASENAME}-client-source ${BOOST_BASENAME}-server-source
 
     BOOST_PACKAGE_BASENAME=boost_${BOOST_VERSION//./_}
 
@@ -116,20 +202,31 @@ for PY_VERSION in ${PY_VERSION_LIST[@]} ; do
     end=$(date +%s)
     echo "Elapsed Time Extracting boost for Python: $(($end-$start)) seconds"
 
-    mkdir -p ${BOOST_BASENAME}-install/include
-    mv ${BOOST_PACKAGE_BASENAME} ${BOOST_BASENAME}-source
+    mkdir -p ${BOOST_BASENAME}-client-install/include
+    mkdir -p ${BOOST_BASENAME}-server-install/include
+    cp -r ${BOOST_PACKAGE_BASENAME} ${BOOST_BASENAME}-client-source
+    mv ${BOOST_PACKAGE_BASENAME} ${BOOST_BASENAME}-server-source
 
-    pushd ${BOOST_BASENAME}-source >/dev/null
+    BOOST_TOOLSET="clang"
+    BOOST_CXXFLAGS="${CARLA_CFLAGS} ${CARLA_CXXFLAGS}"
 
-    BOOST_TOOLSET="clang-10.0"
-    BOOST_CFLAGS="-fPIC -std=c++14 -DBOOST_ERROR_CODE_HEADER_ONLY"
+    pushd ${BOOST_BASENAME}-server-source >/dev/null
+    ./bootstrap.sh \
+        --prefix="../${BOOST_BASENAME}-server-install" \
+        --with-toolset="clang" \
+        --with-libraries=system
+    ./b2 toolset="${BOOST_TOOLSET}" cxxflags="${BOOST_CXXFLAGS} ${CARLA_SERVER_CXXFLAGS}" linkflags="${CARLA_SERVER_LINKER_FLAGS}" --with-system --prefix="../${BOOST_BASENAME}-server-install" -j ${CARLA_BUILD_CONCURRENCY} stage release install
+    popd >/dev/null
+
+    pushd ${BOOST_BASENAME}-client-source >/dev/null
 
     py3="/usr/bin/env python${PY_VERSION}"
     py3_root=`${py3} -c "import sys; print(sys.prefix)"`
     pyv=`$py3 -c "import sys;x='{v[0]}.{v[1]}'.format(v=list(sys.version_info[:2]));sys.stdout.write(x)";`
+
     ./bootstrap.sh \
-        --with-toolset=clang \
-        --prefix=../boost-install \
+        --prefix="../${BOOST_BASENAME}-client-install" \
+        --with-toolset="clang" \
         --with-libraries=python,filesystem,system,program_options \
         --with-python=${py3} --with-python-root=${py3_root}
 
@@ -139,19 +236,16 @@ for PY_VERSION in ${PY_VERSION_LIST[@]} ; do
       echo "using python : ${pyv} : ${py3_root}/bin/python${PY_VERSION} ;" > project-config.jam
     fi
 
-    ./b2 toolset="${BOOST_TOOLSET}" cxxflags="${BOOST_CFLAGS}" --prefix="../${BOOST_BASENAME}-install" -j ${CARLA_BUILD_CONCURRENCY} stage release
-    ./b2 toolset="${BOOST_TOOLSET}" cxxflags="${BOOST_CFLAGS}" --prefix="../${BOOST_BASENAME}-install" -j ${CARLA_BUILD_CONCURRENCY} install
+    ./b2 toolset="${BOOST_TOOLSET}" cxxflags="${BOOST_CXXFLAGS}" --prefix="../${BOOST_BASENAME}-client-install" -j ${CARLA_BUILD_CONCURRENCY} stage release install
 
     popd >/dev/null
 
-    rm -Rf ${BOOST_BASENAME}-source
-    rm ${BOOST_PACKAGE_BASENAME}.tar.gz
+    rm -Rf ${BOOST_BASENAME}-client-source ${BOOST_BASENAME}-server-source
+    rm ${BOOST_PACKAGE_BASENAME}.tar.gz*
 
     # Install boost dependencies
-    mkdir -p "${LIBCARLA_INSTALL_CLIENT_FOLDER}/include/system"
-    mkdir -p "${LIBCARLA_INSTALL_CLIENT_FOLDER}/lib"
-    cp -rf ${BOOST_BASENAME}-install/include/* ${LIBCARLA_INSTALL_CLIENT_FOLDER}/include/system
-    cp -rf ${BOOST_BASENAME}-install/lib/* ${LIBCARLA_INSTALL_CLIENT_FOLDER}/lib
+    cp -rf ${BOOST_BASENAME}-client-install/include/* ${LIBCARLA_INSTALL_CLIENT_FOLDER}/include/system
+    cp -rf ${BOOST_BASENAME}-client-install/lib/* ${LIBCARLA_INSTALL_CLIENT_FOLDER}/lib
 
   fi
 
@@ -159,25 +253,26 @@ done
 
 unset BOOST_BASENAME
 
+
 # ==============================================================================
-# -- Get rpclib and compile it with libc++ and libstdc++ -----------------------
+# -- Get rpclib and compile it for server and client ------
 # ==============================================================================
 
 RPCLIB_PATCH=v2.2.1_c5
-RPCLIB_BASENAME=rpclib-${RPCLIB_PATCH}-${CXX_TAG}
+RPCLIB_BASENAME=rpclib-${RPCLIB_PATCH}
 
-RPCLIB_LIBCXX_INCLUDE=${PWD}/${RPCLIB_BASENAME}-libcxx-install/include
-RPCLIB_LIBCXX_LIBPATH=${PWD}/${RPCLIB_BASENAME}-libcxx-install/lib
-RPCLIB_LIBSTDCXX_INCLUDE=${PWD}/${RPCLIB_BASENAME}-libstdcxx-install/include
-RPCLIB_LIBSTDCXX_LIBPATH=${PWD}/${RPCLIB_BASENAME}-libstdcxx-install/lib
+RPCLIB_SERVER_INCLUDE=${PWD}/${RPCLIB_BASENAME}-server-install/include
+RPCLIB_SERVER_LIBPATH=${PWD}/${RPCLIB_BASENAME}-server-install/lib
+RPCLIB_CLIENT_INCLUDE=${PWD}/${RPCLIB_BASENAME}-client-install/include
+RPCLIB_CLIENT_LIBPATH=${PWD}/${RPCLIB_BASENAME}-client-install/lib
 
-if [[ -d "${RPCLIB_BASENAME}-libcxx-install" && -d "${RPCLIB_BASENAME}-libstdcxx-install" ]] ; then
+if [[ -d "${RPCLIB_BASENAME}-server-install" && -d "${RPCLIB_BASENAME}-client-install" ]] ; then
   log "${RPCLIB_BASENAME} already installed."
 else
   rm -Rf \
       ${RPCLIB_BASENAME}-source \
-      ${RPCLIB_BASENAME}-libcxx-build ${RPCLIB_BASENAME}-libstdcxx-build \
-      ${RPCLIB_BASENAME}-libcxx-install ${RPCLIB_BASENAME}-libstdcxx-install
+      ${RPCLIB_BASENAME}-server-build ${RPCLIB_BASENAME}-client-build \
+      ${RPCLIB_BASENAME}-server-install ${RPCLIB_BASENAME}-client-install
 
   log "Retrieving rpclib."
 
@@ -186,71 +281,61 @@ else
   git clone -b ${RPCLIB_PATCH} https://github.com/carla-simulator/rpclib.git ${RPCLIB_BASENAME}-source
 
   end_download_time=$(date +%s)
-
   echo "Elapsed Time downloading rpclib: $(($end_download_time-$start_download_time)) seconds"
-  log "Building rpclib with libc++."
+  log "Building rpclib for server."
 
   # rpclib does not use any cmake 3.9 feature.
   # As cmake 3.9 is not standard in Ubuntu 16.04, change cmake version to 3.5
   sed -i s/"3.9.0"/"3.5.0"/g ${RPCLIB_BASENAME}-source/CMakeLists.txt
 
-  mkdir -p ${RPCLIB_BASENAME}-libcxx-build
-
-  pushd ${RPCLIB_BASENAME}-libcxx-build >/dev/null
-
+  mkdir -p ${RPCLIB_BASENAME}-server-build
+  pushd ${RPCLIB_BASENAME}-server-build >/dev/null
   cmake -G "Ninja" \
-      -DCMAKE_CXX_FLAGS="-fPIC -std=c++14 -stdlib=libc++ -I${LLVM_INCLUDE} -Wl,-L${LLVM_LIBPATH} -DBOOST_NO_EXCEPTIONS -DASIO_NO_EXCEPTIONS ${UNREAL_HOSTED_CFLAGS}" \
-      -DCMAKE_INSTALL_PREFIX="../${RPCLIB_BASENAME}-libcxx-install" \
+      -DCMAKE_INSTALL_PREFIX="../${RPCLIB_BASENAME}-server-install" \
+      -DCMAKE_TOOLCHAIN_FILE="${CARLA_SERVER_TOOLCHAIN_FILE}" \
       ../${RPCLIB_BASENAME}-source
-
   ninja
-
   ninja install
-
   popd >/dev/null
 
-  log "Building rpclib with libstdc++."
-
-  mkdir -p ${RPCLIB_BASENAME}-libstdcxx-build
-
-  pushd ${RPCLIB_BASENAME}-libstdcxx-build >/dev/null
-
+  log "Building rpclib for client."
+  mkdir -p ${RPCLIB_BASENAME}-client-build
+  pushd ${RPCLIB_BASENAME}-client-build >/dev/null
   cmake -G "Ninja" \
-      -DCMAKE_CXX_FLAGS="-fPIC -std=c++14" \
-      -DCMAKE_INSTALL_PREFIX="../${RPCLIB_BASENAME}-libstdcxx-install" \
+      -DCMAKE_INSTALL_PREFIX="../${RPCLIB_BASENAME}-client-install" \
+      -DCMAKE_TOOLCHAIN_FILE="${CARLA_CLIENT_TOOLCHAIN_FILE}" \
       ../${RPCLIB_BASENAME}-source
-
   ninja
-
   ninja install
-
   popd >/dev/null
 
-  rm -Rf ${RPCLIB_BASENAME}-source ${RPCLIB_BASENAME}-libcxx-build ${RPCLIB_BASENAME}-libstdcxx-build
-
+  rm -Rf ${RPCLIB_BASENAME}-source ${RPCLIB_BASENAME}-server-build ${RPCLIB_BASENAME}-client-build
 fi
 
 unset RPCLIB_BASENAME
 
+cp -p -r ${RPCLIB_SERVER_INCLUDE}/* ${LIBCARLA_INSTALL_SERVER_FOLDER}/include/
+cp -p ${RPCLIB_SERVER_LIBPATH}/*.a ${LIBCARLA_INSTALL_SERVER_FOLDER}/lib
+
 # ==============================================================================
-# -- Get GTest and compile it with libc++ --------------------------------------
+# -- Get GTest and compile it for server --------------------------------------
 # ==============================================================================
 
 GTEST_VERSION=1.8.1
-GTEST_BASENAME=gtest-${GTEST_VERSION}-${CXX_TAG}
+GTEST_BASENAME=gtest-${GTEST_VERSION}
 
-GTEST_LIBCXX_INCLUDE=${PWD}/${GTEST_BASENAME}-libcxx-install/include
-GTEST_LIBCXX_LIBPATH=${PWD}/${GTEST_BASENAME}-libcxx-install/lib
-GTEST_LIBSTDCXX_INCLUDE=${PWD}/${GTEST_BASENAME}-libstdcxx-install/include
-GTEST_LIBSTDCXX_LIBPATH=${PWD}/${GTEST_BASENAME}-libstdcxx-install/lib
+GTEST_SERVER_INCLUDE=${PWD}/${GTEST_BASENAME}-server-install/include
+GTEST_SERVER_LIBPATH=${PWD}/${GTEST_BASENAME}-server-install/lib
+GTEST_CLIENT_INCLUDE=${PWD}/${GTEST_BASENAME}-client-install/include
+GTEST_CLIENT_LIBPATH=${PWD}/${GTEST_BASENAME}-client-install/lib
 
-if [[ -d "${GTEST_BASENAME}-libcxx-install" && -d "${GTEST_BASENAME}-libstdcxx-install" ]] ; then
+if [[ -d "${GTEST_BASENAME}-server-install" && -d "${GTEST_BASENAME}-client-install" ]] ; then
   log "${GTEST_BASENAME} already installed."
 else
   rm -Rf \
       ${GTEST_BASENAME}-source \
-      ${GTEST_BASENAME}-libcxx-build ${GTEST_BASENAME}-libstdcxx-build \
-      ${GTEST_BASENAME}-libcxx-install ${GTEST_BASENAME}-libstdcxx-install
+      ${GTEST_BASENAME}-server-build ${GTEST_BASENAME}-client-build \
+      ${GTEST_BASENAME}-server-install ${GTEST_BASENAME}-client-install
 
   log "Retrieving Google Test."
 
@@ -261,63 +346,52 @@ else
   end_download_time=$(date +%s)
   echo "Elapsed Time downloading rpclib: $(($end_download_time-$start_download_time)) seconds"
 
-  log "Building Google Test with libc++."
+  log "Building Google Test for server."
 
-  mkdir -p ${GTEST_BASENAME}-libcxx-build
-
-  pushd ${GTEST_BASENAME}-libcxx-build >/dev/null
+  mkdir -p ${GTEST_BASENAME}-server-build
+  pushd ${GTEST_BASENAME}-server-build >/dev/null
 
   cmake -G "Ninja" \
-      -DCMAKE_CXX_FLAGS="-std=c++14 -stdlib=libc++ -I${LLVM_INCLUDE} -Wl,-L${LLVM_LIBPATH} -DBOOST_NO_EXCEPTIONS -fno-exceptions ${UNREAL_HOSTED_CFLAGS}" \
-      -DCMAKE_INSTALL_PREFIX="../${GTEST_BASENAME}-libcxx-install" \
+      -DCMAKE_INSTALL_PREFIX="../${GTEST_BASENAME}-server-install" \
+      -DCMAKE_TOOLCHAIN_FILE="${CARLA_SERVER_TOOLCHAIN_FILE}" \
       ../${GTEST_BASENAME}-source
-
   ninja
-
   ninja install
-
   popd >/dev/null
 
-  log "Building Google Test with libstdc++."
-
-  mkdir -p ${GTEST_BASENAME}-libstdcxx-build
-
-  pushd ${GTEST_BASENAME}-libstdcxx-build >/dev/null
-
+  log "Building Google Test for client."
+  mkdir -p ${GTEST_BASENAME}-client-build
+  pushd ${GTEST_BASENAME}-client-build >/dev/null
   cmake -G "Ninja" \
-      -DCMAKE_CXX_FLAGS="-std=c++14" \
-      -DCMAKE_INSTALL_PREFIX="../${GTEST_BASENAME}-libstdcxx-install" \
+      -DCMAKE_INSTALL_PREFIX="../${GTEST_BASENAME}-client-install" \
+      -DCMAKE_TOOLCHAIN_FILE="${CARLA_CLIENT_TOOLCHAIN_FILE}" \
       ../${GTEST_BASENAME}-source
-
   ninja
-
   ninja install
-
   popd >/dev/null
 
-  rm -Rf ${GTEST_BASENAME}-source ${GTEST_BASENAME}-libcxx-build ${GTEST_BASENAME}-libstdcxx-build
-
+  rm -Rf ${GTEST_BASENAME}-source ${GTEST_BASENAME}-server-build ${GTEST_BASENAME}-client-build
 fi
 
 unset GTEST_BASENAME
 
 # ==============================================================================
-# -- Get Recast&Detour and compile it with libc++ ------------------------------
+# -- Get Recast&Detour and compile it for client ------------------------------
 # ==============================================================================
 
-RECAST_BASENAME=recast-${CXX_TAG}
+RECAST_BASENAME=recast
 
-RECAST_INCLUDE=${PWD}/${RECAST_BASENAME}-install/include
-RECAST_LIBPATH=${PWD}/${RECAST_BASENAME}-install/lib
+RECAST_CLIENT_INCLUDE=${PWD}/${RECAST_BASENAME}-client-install/include
+RECAST_CLIENT_LIBPATH=${PWD}/${RECAST_BASENAME}-client-install/lib
 
-if [[ -d "${RECAST_BASENAME}-install" &&
-      -f "${RECAST_BASENAME}-install/bin/RecastBuilder" ]] ; then
+if [[ -d "${RECAST_BASENAME}-client-install" &&
+      -f "${RECAST_BASENAME}-client-install/bin/RecastBuilder" ]] ; then
   log "${RECAST_BASENAME} already installed."
 else
   rm -Rf \
       ${RECAST_BASENAME}-source \
-      ${RECAST_BASENAME}-build \
-      ${RECAST_BASENAME}-install
+      ${RECAST_BASENAME}-client-build \
+      ${RECAST_BASENAME}-client-install
 
   log "Retrieving Recast & Detour"
 
@@ -332,33 +406,26 @@ else
 
   popd >/dev/null
 
-  log "Building Recast & Detour with libc++."
-
-  mkdir -p ${RECAST_BASENAME}-build
-
-  pushd ${RECAST_BASENAME}-build >/dev/null
-
+  log "Building Recast & Detour for client."
+  mkdir -p ${RECAST_BASENAME}-client-build
+  pushd ${RECAST_BASENAME}-client-build >/dev/null
   cmake -G "Ninja" \
-      -DCMAKE_CXX_FLAGS="-std=c++14 -fPIC" \
-      -DCMAKE_INSTALL_PREFIX="../${RECAST_BASENAME}-install" \
+      -DCMAKE_INSTALL_PREFIX="../${RECAST_BASENAME}-client-install" \
+      -DCMAKE_TOOLCHAIN_FILE="${CARLA_CLIENT_TOOLCHAIN_FILE}" \
       -DRECASTNAVIGATION_DEMO=False \
       -DRECASTNAVIGATION_TEST=False \
       ../${RECAST_BASENAME}-source
-
   ninja
-
   ninja install
-
   popd >/dev/null
 
-  rm -Rf ${RECAST_BASENAME}-source ${RECAST_BASENAME}-build
-
+  rm -Rf ${RECAST_BASENAME}-source ${RECAST_BASENAME}-client-build
 fi
 
 # make sure the RecastBuilder is corrctly copied
-RECAST_INSTALL_DIR="${CARLA_BUILD_FOLDER}/../Util/DockerUtils/dist"
-if [[ ! -f "${RECAST_INSTALL_DIR}/RecastBuilder" ]]; then
-  cp "${RECAST_BASENAME}-install/bin/RecastBuilder" "${RECAST_INSTALL_DIR}/"
+RECAST_INSTALL_CLIENT_DIR="${CARLA_BUILD_FOLDER}/../Util/DockerUtils/dist"
+if [[ ! -f "${RECAST_INSTALL_CLIENT_DIR}/RecastBuilder" ]]; then
+  cp "${RECAST_BASENAME}-client-install/bin/RecastBuilder" "${RECAST_INSTALL_CLIENT_DIR}/"
 fi
 
 unset RECAST_BASENAME
@@ -370,14 +437,18 @@ unset RECAST_BASENAME
 LIBPNG_VERSION=1.6.37
 LIBPNG_REPO=https://sourceforge.net/projects/libpng/files/libpng16/${LIBPNG_VERSION}/libpng-${LIBPNG_VERSION}.tar.xz
 LIBPNG_BASENAME=libpng-${LIBPNG_VERSION}
-LIBPNG_INSTALL=${LIBPNG_BASENAME}-install
 
-LIBPNG_INCLUDE=${PWD}/${LIBPNG_BASENAME}-install/include/
-LIBPNG_LIBPATH=${PWD}/${LIBPNG_BASENAME}-install/lib
+LIBPNG_CLIENT_INCLUDE=${PWD}/${LIBPNG_BASENAME}-client-install/include/
+LIBPNG_CLIENT_LIBPATH=${PWD}/${LIBPNG_BASENAME}-client-install/lib
 
-if [[ -d ${LIBPNG_INSTALL} ]] ; then
+if [[ -d ${LIBPNG_BASENAME}-client-install ]] ; then
   log "Libpng already installed."
 else
+  rm -Rf \
+      ${LIBPNG_BASENAME}-source \
+      ${LIBPNG_BASENAME}-client-build \
+      ${LIBPNG_BASENAME}-client-install
+
   log "Retrieving libpng."
 
   start=$(date +%s)
@@ -393,15 +464,16 @@ else
 
   mv ${LIBPNG_BASENAME} ${LIBPNG_BASENAME}-source
 
-  pushd ${LIBPNG_BASENAME}-source >/dev/null
 
-  ./configure --prefix=${CARLA_BUILD_FOLDER}/${LIBPNG_INSTALL}
+  mkdir ${LIBPNG_BASENAME}-client-build
+  pushd ${LIBPNG_BASENAME}-client-build >/dev/null
+  ../${LIBPNG_BASENAME}-source/configure --prefix=${PWD}/../${LIBPNG_BASENAME}-client-install/
+  make
   make install
-
   popd >/dev/null
 
   rm -Rf libpng-${LIBPNG_VERSION}.tar.xz
-  rm -Rf ${LIBPNG_BASENAME}-source
+  rm -Rf ${LIBPNG_BASENAME}-source ${LIBPNG_BASENAME}-client-build
 fi
 
 # ==============================================================================
@@ -414,15 +486,17 @@ XERCESC_BASENAME=xerces-c-${XERCESC_VERSION}
 XERCESC_TEMP_FOLDER=${XERCESC_BASENAME}
 XERCESC_REPO=https://archive.apache.org/dist/xerces/c/3/sources/xerces-c-${XERCESC_VERSION}.tar.gz
 
-XERCESC_SRC_DIR=${XERCESC_BASENAME}-source
-XERCESC_INSTALL_DIR=${XERCESC_BASENAME}-install
-XERCESC_INSTALL_SERVER_DIR=${XERCESC_BASENAME}-install-server
-XERCESC_LIB=${XERCESC_INSTALL_DIR}/lib/libxerces-c.a
-XERCESC_SERVER_LIB=${XERCESC_INSTALL_SERVER_DIR}/lib/libxerces-c.a
+XERCESC_CLIENT_LIB=${XERCESC_BASENAME}-client-install/lib/libxerces-c.a
+XERCESC_SERVER_LIB=${XERCESC_BASENAME}-server-install/lib/libxerces-c.a
 
-if [[ -d ${XERCESC_INSTALL_DIR} &&  -d ${XERCESC_INSTALL_SERVER_DIR} ]] ; then
+if [[ -d ${XERCESC_BASENAME}-client-install &&  -d ${XERCESC_BASENAME}-server-install ]] ; then
   log "Xerces-c already installed."
 else
+  rm -Rf \
+      ${XERCESC_BASENAME}-source \
+      ${XERCESC_BASENAME}-server-build ${XERCESC_BASENAME}-client-build \
+      ${XERCESC_BASENAME}-server-install ${XERCESC_BASENAME}-client-install
+
   log "Retrieving xerces-c."
   start=$(date +%s)
   wget ${XERCESC_REPO}
@@ -444,50 +518,44 @@ else
   end=$(date +%s)
   echo "Elapsed Time Extracting xerces-c: $(($end-$start)) seconds"
 
-  mv ${XERCESC_BASENAME} ${XERCESC_SRC_DIR}
-  mkdir -p ${XERCESC_INSTALL_DIR}
-  mkdir -p ${XERCESC_SRC_DIR}/build
-
-  pushd ${XERCESC_SRC_DIR}/build >/dev/null
+  mv ${XERCESC_BASENAME} ${XERCESC_BASENAME}-source
 
+  log "Building xerces-c for server."
+  mkdir -p ${XERCESC_BASENAME}-server-build
+  pushd ${XERCESC_BASENAME}-server-build >/dev/null
   cmake -G "Ninja" \
-      -DCMAKE_CXX_FLAGS="-std=c++14 -fPIC -w" \
-      -DCMAKE_INSTALL_PREFIX="../../${XERCESC_INSTALL_DIR}" \
+      -DCMAKE_INSTALL_PREFIX="../${XERCESC_BASENAME}-server-install" \
+      -DCMAKE_TOOLCHAIN_FILE="${CARLA_SERVER_TOOLCHAIN_FILE}" \
       -DCMAKE_BUILD_TYPE=Release \
       -DBUILD_SHARED_LIBS=OFF \
       -Dtranscoder=gnuiconv \
       -Dnetwork=OFF \
-      ..
+      ../${XERCESC_BASENAME}-source
   ninja
   ninja install
-
   popd >/dev/null
 
-  mkdir -p ${XERCESC_INSTALL_SERVER_DIR}
-
-  pushd ${XERCESC_SRC_DIR}/build >/dev/null
+  log "Building xerces-c for client."
+  mkdir -p ${XERCESC_BASENAME}-client-build
+  pushd ${XERCESC_BASENAME}-client-build >/dev/null
 
   cmake -G "Ninja" \
-      -DCMAKE_CXX_FLAGS="-std=c++14 -stdlib=libc++ -fPIC -w -I${LLVM_INCLUDE} -L${LLVM_LIBPATH}" \
-      -DCMAKE_INSTALL_PREFIX="../../${XERCESC_INSTALL_SERVER_DIR}" \
+      -DCMAKE_INSTALL_PREFIX="../${XERCESC_BASENAME}-client-install" \
+      -DCMAKE_TOOLCHAIN_FILE="${CARLA_CLIENT_TOOLCHAIN_FILE}" \
       -DCMAKE_BUILD_TYPE=Release \
       -DBUILD_SHARED_LIBS=OFF \
       -Dtranscoder=gnuiconv \
       -Dnetwork=OFF \
-      ..
+      ../${XERCESC_BASENAME}-source
   ninja
   ninja install
-
   popd >/dev/null
 
   rm -Rf ${XERCESC_BASENAME}.tar.gz
-  rm -Rf ${XERCESC_SRC_DIR}
+  rm -Rf ${XERCESC_BASENAME}-source ${XERCESC_BASENAME}-client-build ${XERCESC_BASENAME}-server-build
 fi
 
-mkdir -p ${LIBCARLA_INSTALL_CLIENT_FOLDER}/lib/
-cp ${XERCESC_LIB} ${LIBCARLA_INSTALL_CLIENT_FOLDER}/lib/
-
-mkdir -p ${LIBCARLA_INSTALL_SERVER_FOLDER}/lib/
+cp ${XERCESC_CLIENT_LIB} ${LIBCARLA_INSTALL_CLIENT_FOLDER}/lib/
 cp -p ${XERCESC_SERVER_LIB} ${LIBCARLA_INSTALL_SERVER_FOLDER}/lib/
 
 # ==============================================================================
@@ -498,12 +566,9 @@ EIGEN_VERSION=3.1.0
 EIGEN_REPO=https://gitlab.com/libeigen/eigen/-/archive/${EIGEN_VERSION}/eigen-${EIGEN_VERSION}.tar.gz
 EIGEN_BASENAME=eigen-${EIGEN_VERSION}
 
-EIGEN_SRC_DIR=eigen-${EIGEN_VERSION}-src
-EIGEN_INSTALL_DIR=eigen-${EIGEN_VERSION}-install
-EIGEN_INCLUDE=${EIGEN_INSTALL_DIR}/include
+EIGEN_INCLUDE=${EIGEN_BASENAME}-server-install/include
 
-
-if [[ -d ${EIGEN_INSTALL_DIR} ]] ; then
+if [[ -d ${EIGEN_BASENAME}-server-install ]] ; then
   log "Eigen already installed."
 else
   log "Retrieving Eigen."
@@ -516,21 +581,31 @@ else
   log "Extracting Eigen."
   start=$(date +%s)
   tar -xzf ${EIGEN_BASENAME}.tar.gz
+
   end=$(date +%s)
   echo "Elapsed Time Extracting EIGEN: $(($end-$start)) seconds"
 
-  mv ${EIGEN_BASENAME} ${EIGEN_SRC_DIR}
+  mv ${EIGEN_BASENAME} ${EIGEN_BASENAME}-source
+  # patch some files
+  cp $CARLA_ROOT_FOLDER/Util/Patches/Eigen${EIGEN_VERSION}/Functors.h "${EIGEN_BASENAME}-source/Eigen/src/Core/Functors.h"
+  cp $CARLA_ROOT_FOLDER/Util/Patches/Eigen${EIGEN_VERSION}/VectorBlock.h "${EIGEN_BASENAME}-source/Eigen/src/Core/VectorBlock.h"
+  cp $CARLA_ROOT_FOLDER/Util/Patches/Eigen${EIGEN_VERSION}/PacketMath.h "${EIGEN_BASENAME}-source/Eigen/src/Core/arch/SSE/PacketMath.h"
+  cp $CARLA_ROOT_FOLDER/Util/Patches/Eigen${EIGEN_VERSION}/SelfadjointMatrixVector.h "${EIGEN_BASENAME}-source/Eigen/src/Core/products/SelfadjointMatrixVector.h"
+  cp $CARLA_ROOT_FOLDER/Util/Patches/Eigen${EIGEN_VERSION}/Macros.h "${EIGEN_BASENAME}-source/Eigen/src/Core/util/Macros.h"
+  cp $CARLA_ROOT_FOLDER/Util/Patches/Eigen${EIGEN_VERSION}/ArrayCwiseUnaryOps.h "${EIGEN_BASENAME}-source/Eigen/src/plugins/ArrayCwiseUnaryOps.h"
+  cp $CARLA_ROOT_FOLDER/Util/Patches/Eigen${EIGEN_VERSION}/MatrixCwiseUnaryOps.h "${EIGEN_BASENAME}-source/Eigen/src/plugins/MatrixCwiseUnaryOps.h"
+
   mkdir -p ${EIGEN_INCLUDE}/unsupported
-  mv ${EIGEN_SRC_DIR}/Eigen ${EIGEN_INCLUDE}
-  mv ${EIGEN_SRC_DIR}/unsupported/Eigen ${EIGEN_INCLUDE}/unsupported/Eigen
+  mv ${EIGEN_BASENAME}-source/Eigen ${EIGEN_INCLUDE}
+  mv ${EIGEN_BASENAME}-source/unsupported/Eigen ${EIGEN_INCLUDE}/unsupported/Eigen
 
   rm -Rf ${EIGEN_BASENAME}.tar.gz
-  rm -Rf ${EIGEN_SRC_DIR}
+  rm -Rf ${EIGEN_BASENAME}-source
 fi
 
-mkdir -p ${LIBCARLA_INSTALL_SERVER_FOLDER}/include/
 cp -p -r ${EIGEN_INCLUDE}/* ${LIBCARLA_INSTALL_SERVER_FOLDER}/include/
 
+
 if ${USE_CHRONO} ; then
 
   # ==============================================================================
@@ -541,12 +616,9 @@ if ${USE_CHRONO} ; then
   EIGEN_REPO=https://gitlab.com/libeigen/eigen/-/archive/3.3.7/eigen-3.3.7.tar.gz
   EIGEN_BASENAME=eigen-${EIGEN_VERSION}
 
-  EIGEN_SRC_DIR=eigen-${EIGEN_VERSION}-src
-  EIGEN_INSTALL_DIR=eigen-${EIGEN_VERSION}-install
-  EIGEN_INCLUDE=${EIGEN_INSTALL_DIR}/include
+  EIGEN_INCLUDE=${EIGEN_BASENAME}-server-install/include
 
-
-  if [[ -d ${EIGEN_INSTALL_DIR} ]] ; then
+  if [[ -d ${EIGEN_BASENAME}-server-install ]] ; then
     log "Eigen already installed."
   else
     log "Retrieving Eigen."
@@ -563,61 +635,58 @@ if ${USE_CHRONO} ; then
     end=$(date +%s)
     echo "Elapsed Time Extracting for Eigen: $(($end-$start)) seconds"
 
-    mv ${EIGEN_BASENAME} ${EIGEN_SRC_DIR}
+    mv ${EIGEN_BASENAME} ${EIGEN_BASENAME}-source
     mkdir -p ${EIGEN_INCLUDE}/unsupported
-    mv ${EIGEN_SRC_DIR}/Eigen ${EIGEN_INCLUDE}
-    mv ${EIGEN_SRC_DIR}/unsupported/Eigen ${EIGEN_INCLUDE}/unsupported/Eigen
+    mv ${EIGEN_BASENAME}-source/Eigen ${EIGEN_INCLUDE}
+    mv ${EIGEN_BASENAME}-source/unsupported/Eigen ${EIGEN_INCLUDE}/unsupported/Eigen
 
     rm -Rf ${EIGEN_BASENAME}.tar.gz
-    rm -Rf ${EIGEN_SRC_DIR}
+    rm -Rf ${EIGEN_BASENAME}-source
+
+    cp -p -r ${EIGEN_INCLUDE}/* ${LIBCARLA_INSTALL_SERVER_FOLDER}/include/
   fi
 
-  mkdir -p ${LIBCARLA_INSTALL_SERVER_FOLDER}/include/
-  cp -p -r ${EIGEN_INCLUDE}/* ${LIBCARLA_INSTALL_SERVER_FOLDER}/include/
 
   # ==============================================================================
-  # -- Get Chrono and compile it with libc++ -------------------------------------
+  # -- Get Chrono and compile it for server -------------------------------------
   # ==============================================================================
 
   CHRONO_TAG=6.0.0
   # CHRONO_TAG=develop
   CHRONO_REPO=https://github.com/projectchrono/chrono.git
+  CHRONO_BASENAME=chrono
 
   CHRONO_SRC_DIR=chrono-source
-  CHRONO_INSTALL_DIR=chrono-install
+  CHRONO_INSTALL_CLIENT_DIR=chrono-install
 
-  if [[ -d ${CHRONO_INSTALL_DIR} ]] ; then
+  if [[ -d ${CHRONO_BASENAME}-server-install ]] ; then
     log "chrono library already installed."
   else
     log "Retrieving chrono library."
     start=$(date +%s)
-    git clone --depth 1 --branch ${CHRONO_TAG} ${CHRONO_REPO} ${CHRONO_SRC_DIR}
+    git clone --depth 1 --branch ${CHRONO_TAG} ${CHRONO_REPO} ${CHRONO_BASENAME}-source
     end=$(date +%s)
     echo "Elapsed Time dowloading chrono: $(($end-$start)) seconds"
 
-    mkdir -p ${CHRONO_SRC_DIR}/build
-
-    pushd ${CHRONO_SRC_DIR}/build >/dev/null
-
+    mkdir -p ${CHRONO_BASENAME}-server-build
+    pushd ${CHRONO_SRC_DIR}/server-build >/dev/null
     cmake -G "Ninja" \
-        -DCMAKE_CXX_FLAGS="-fPIC -std=c++14 -stdlib=libc++ -I${LLVM_INCLUDE} -L${LLVM_LIBPATH} -Wno-unused-command-line-argument ${UNREAL_HOSTED_CFLAGS}" \
-        -DEIGEN3_INCLUDE_DIR="../../${EIGEN_INCLUDE}" \
-        -DCMAKE_INSTALL_PREFIX="../../${CHRONO_INSTALL_DIR}" \
+        -DCMAKE_CXX_FLAGS="-Wno-unused-command-line-argument" \
+        -DCMAKE_INSTALL_PREFIX="../${CHRONO_BASENAME}-server-install" \
+        -DCMAKE_TOOLCHAIN_FILE="${CARLA_SERVER_TOOLCHAIN_FILE}" \
         -DCMAKE_BUILD_TYPE=Release \
         -DENABLE_MODULE_VEHICLE=ON \
+        -DEIGEN3_INCLUDE_DIR="../../${EIGEN_INCLUDE}" \
         ..
     ninja
     ninja install
-
     popd >/dev/null
 
     rm -Rf ${CHRONO_SRC_DIR}
   fi
 
-  mkdir -p ${LIBCARLA_INSTALL_SERVER_FOLDER}/lib/
-  mkdir -p ${LIBCARLA_INSTALL_SERVER_FOLDER}/include/
-  cp -p ${CHRONO_INSTALL_DIR}/lib/*.so ${LIBCARLA_INSTALL_SERVER_FOLDER}/lib/
-  cp -p -r ${CHRONO_INSTALL_DIR}/include/* ${LIBCARLA_INSTALL_SERVER_FOLDER}/include/
+  cp -p ${CHRONO_INSTALL_CLIENT_DIR}/lib/*.a ${LIBCARLA_INSTALL_SERVER_FOLDER}/lib/
+  cp -p -r ${CHRONO_INSTALL_CLIENT_DIR}/include/* ${LIBCARLA_INSTALL_SERVER_FOLDER}/include/
 fi
 
 # ==============================================================================
@@ -626,19 +695,24 @@ fi
 
 SQLITE_VERSION=sqlite-autoconf-3340100
 SQLITE_REPO=https://www.sqlite.org/2021/${SQLITE_VERSION}.tar.gz
-
 SQLITE_TAR=${SQLITE_VERSION}.tar.gz
-SQLITE_SOURCE_DIR=sqlite-src
-SQLITE_INSTALL_DIR=sqlite-install
+SQLITE_BASENAME=sqlite
 
-SQLITE_INCLUDE_DIR=${PWD}/${SQLITE_INSTALL_DIR}/include
-SQLITE_LIB=${PWD}/${SQLITE_INSTALL_DIR}/lib/libsqlite3.a
-SQLITE_FULL_LIB=${PWD}/${SQLITE_INSTALL_DIR}/lib/
-SQLITE_EXE=${PWD}/${SQLITE_INSTALL_DIR}/bin/sqlite3
+SQLITE_SERVER_INCLUDE_DIR=${PWD}/${SQLITE_BASENAME}-server-install/include
+SQLITE_SERVER_LIB=${PWD}/${SQLITE_BASENAME}-server-install/lib/libsqlite3.a
+SQLITE_CLIENT_INCLUDE_DIR=${PWD}/${SQLITE_BASENAME}-client-install/include
+SQLITE_CLIENT_LIB=${PWD}/${SQLITE_BASENAME}-client-install/lib/libsqlite3.a
+SQLITE_EXE=${PWD}/${SQLITE_BASENAME}-client-install/bin/sqlite3
 
-if [[ -d ${SQLITE_INSTALL_DIR} ]] ; then
+if [[ -d ${SQLITE_BASENAME}-server-install && -d ${SQLITE_BASENAME}-client-install ]] ; then
   log "Sqlite already installed."
 else
+  rm -Rf \
+      ${SQLITE_BASENAME}-source \
+      ${SQLITE_BASENAME}-build  \
+      ${SQLITE_BASENAME}-client-install \
+      ${SQLITE_BASENAME}-server-install
+
   log "Retrieving Sqlite3"
 
   start=$(date +%s)
@@ -653,108 +727,94 @@ else
   end=$(date +%s)
   echo "Elapsed Time Extracting for SQlite: $(($end-$start)) seconds"
 
-  mv ${SQLITE_VERSION} ${SQLITE_SOURCE_DIR}
+  mv ${SQLITE_VERSION} ${SQLITE_BASENAME}-source
 
-  mkdir ${SQLITE_INSTALL_DIR}
-
-  pushd ${SQLITE_SOURCE_DIR} >/dev/null
-
-  export CFLAGS="-fPIC -w"
-  ./configure --prefix=${PWD}/../sqlite-install/
-  make
+  mkdir ${SQLITE_BASENAME}-client-build
+  pushd ${SQLITE_BASENAME}-client-build >/dev/null
+  ../${SQLITE_BASENAME}-source/configure CFLAGS="${CARLA_CFLAGS} -w" --prefix=${PWD}/../${SQLITE_BASENAME}-client-install/
   make install
-
   popd >/dev/null
 
+  mkdir ${SQLITE_BASENAME}-server-build
+  pushd ${SQLITE_BASENAME}-server-build >/dev/null
+  ../${SQLITE_BASENAME}-source/configure CFLAGS="${CARLA_SERVER_CFLAGS} -w" --prefix=${PWD}/../${SQLITE_BASENAME}-server-install/
+  make install
+  popd >/dev/null
+
+
   rm -Rf ${SQLITE_TAR}
-  rm -Rf ${SQLITE_SOURCE_DIR}
+  rm -Rf ${SQLITE_BASENAME}-source ${SQLITE_BASENAME}-client-build ${SQLITE_BASENAME}-server-build
 fi
 
-mkdir -p ${LIBCARLA_INSTALL_CLIENT_FOLDER}/lib/
-cp ${SQLITE_LIB} ${LIBCARLA_INSTALL_CLIENT_FOLDER}/lib/
-
-mkdir -p ${LIBCARLA_INSTALL_SERVER_FOLDER}/lib/
-cp -p -r ${SQLITE_FULL_LIB} ${LIBCARLA_INSTALL_SERVER_FOLDER}
+cp ${SQLITE_CLIENT_LIB} ${LIBCARLA_INSTALL_CLIENT_FOLDER}/lib/
+cp -p -r ${SQLITE_SERVER_LIB}* ${LIBCARLA_INSTALL_SERVER_FOLDER}/lib/
 
 # ==============================================================================
 # -- Get and compile PROJ ------------------------------------------------------
 # ==============================================================================
 
-PROJ_VERSION=proj-7.2.1
-PROJ_REPO=https://download.osgeo.org/proj/${PROJ_VERSION}.tar.gz
+PROJ_VERSION=7.2
+PROJ_REPO=https://github.com/OSGeo/PROJ
 
-PROJ_TAR=${PROJ_VERSION}.tar.gz
-PROJ_SRC_DIR=proj-src
-PROJ_INSTALL_DIR=proj-install
-PROJ_INSTALL_SERVER_DIR=proj-install-server
-PROJ_INSTALL_DIR_FULL=${PWD}/${PROJ_INSTALL_DIR}
-PROJ_INSTALL_SERVER_DIR_FULL=${PWD}/${PROJ_INSTALL_SERVER_DIR}
-PROJ_LIB=${PROJ_INSTALL_DIR_FULL}/lib/libproj.a
-PROJ_SERVER_LIB=${PROJ_INSTALL_SERVER_DIR_FULL}/lib/libproj.a
+PROJ_BASENAME=proj
+PROJ_CLIENT_LIBPATH=${PWD}/${PROJ_BASENAME}-client-install/lib
+PROJ_CLIENT_LIB=${PROJ_CLIENT_LIBPATH}/libproj.a
+PROJ_SERVER_LIB=${PWD}/${PROJ_BASENAME}-server-install/lib/libproj.a
+PROJ_CLIENT_INCLUDE=${PWD}/${PROJ_BASENAME}-client-install/include
 
-if [[ -d ${PROJ_INSTALL_DIR} && -d ${PROJ_INSTALL_SERVER_DIR_FULL} ]] ; then
+if [[ -d ${PROJ_BASENAME}-client-install && -d ${PROJ_BASENAME}-server-install ]] ; then
   log "PROJ already installed."
 else
+  rm -Rf \
+      ${PROJ_BASENAME}-source \
+      ${PROJ_BASENAME}-server-build ${PROJ_BASENAME}-client-build \
+      ${PROJ_BASENAME}-server-install ${PROJ_BASENAME}-client-install
+
   log "Retrieving PROJ"
 
   start=$(date +%s)
-  wget ${PROJ_REPO}
+  git clone -b ${PROJ_VERSION} --depth 1 ${PROJ_REPO} ${PROJ_BASENAME}-source
   end=$(date +%s)
   echo "Elapsed Time: $(($end-$start)) seconds"
 
-  log "Extracting PROJ"
-  start=$(date +%s)
-  tar -xzf ${PROJ_TAR}
-  end=$(date +%s)
-  echo "Elapsed Time Extracting for PROJ: $(($end-$start)) seconds"
-
-  mv ${PROJ_VERSION} ${PROJ_SRC_DIR}
-
-  mkdir -p ${PROJ_SRC_DIR}/build
-  mkdir -p ${PROJ_INSTALL_DIR}
-
-  pushd ${PROJ_SRC_DIR}/build >/dev/null
-
-  cmake -G "Ninja" .. \
-      -DCMAKE_CXX_FLAGS="-std=c++14 -fPIC" \
-      -DSQLITE3_INCLUDE_DIR=${SQLITE_INCLUDE_DIR} -DSQLITE3_LIBRARY=${SQLITE_LIB} \
+  mkdir -p ${PROJ_BASENAME}-client-build
+  pushd ${PROJ_BASENAME}-client-build >/dev/null
+  cmake -G "Ninja" \
+      -DCMAKE_INSTALL_PREFIX=../${PROJ_BASENAME}-client-install \
+      -DCMAKE_TOOLCHAIN_FILE="${CARLA_CLIENT_TOOLCHAIN_FILE}" \
+      -DSQLITE3_INCLUDE_DIR=${SQLITE_CLIENT_INCLUDE_DIR} -DSQLITE3_LIBRARY=${SQLITE_CLIENT_LIB} \
       -DEXE_SQLITE3=${SQLITE_EXE} \
       -DENABLE_TIFF=OFF -DENABLE_CURL=OFF -DBUILD_SHARED_LIBS=OFF -DBUILD_PROJSYNC=OFF \
       -DCMAKE_BUILD_TYPE=Release -DBUILD_PROJINFO=OFF \
       -DBUILD_CCT=OFF -DBUILD_CS2CS=OFF -DBUILD_GEOD=OFF -DBUILD_GIE=OFF \
       -DBUILD_PROJ=OFF -DBUILD_TESTING=OFF \
-      -DCMAKE_INSTALL_PREFIX=${PROJ_INSTALL_DIR_FULL}
+      ../${PROJ_BASENAME}-source
   ninja
   ninja install
-
   popd >/dev/null
 
-  mkdir -p ${PROJ_INSTALL_SERVER_DIR}
-
-  pushd ${PROJ_SRC_DIR}/build >/dev/null
-
-  cmake -G "Ninja" .. \
-      -DCMAKE_CXX_FLAGS="-std=c++14 -fPIC -stdlib=libc++ -I${LLVM_INCLUDE} -Wl,-L${LLVM_LIBPATH}"  \
-      -DSQLITE3_INCLUDE_DIR=${SQLITE_INCLUDE_DIR} -DSQLITE3_LIBRARY=${SQLITE_LIB} \
+  mkdir -p ${PROJ_BASENAME}-server-build
+  pushd ${PROJ_BASENAME}-server-build >/dev/null
+  cmake -G "Ninja" \
+      -DCMAKE_INSTALL_PREFIX=../${PROJ_BASENAME}-server-install \
+      -DCMAKE_TOOLCHAIN_FILE="${CARLA_SERVER_TOOLCHAIN_FILE}" \
+      -DSQLITE3_INCLUDE_DIR=${SQLITE_SERVER_INCLUDE_DIR} -DSQLITE3_LIBRARY=${SQLITE_SERVER_LIB} \
       -DEXE_SQLITE3=${SQLITE_EXE} \
       -DENABLE_TIFF=OFF -DENABLE_CURL=OFF -DBUILD_SHARED_LIBS=OFF -DBUILD_PROJSYNC=OFF \
       -DCMAKE_BUILD_TYPE=Release -DBUILD_PROJINFO=OFF \
       -DBUILD_CCT=OFF -DBUILD_CS2CS=OFF -DBUILD_GEOD=OFF -DBUILD_GIE=OFF \
       -DBUILD_PROJ=OFF -DBUILD_TESTING=OFF \
-      -DCMAKE_INSTALL_PREFIX=${PROJ_INSTALL_SERVER_DIR_FULL}
+      ../${PROJ_BASENAME}-source
   ninja
   ninja install
-
   popd >/dev/null
 
   rm -Rf ${PROJ_TAR}
-  rm -Rf ${PROJ_SRC_DIR}
+  rm -Rf ${PROJ_BASENAME}-source ${PROJ_BASENAME}-client-build ${PROJ_BASENAME}-server-build
 
 fi
 
-cp ${PROJ_LIB} ${LIBCARLA_INSTALL_CLIENT_FOLDER}/lib/
-
-mkdir -p ${LIBCARLA_INSTALL_SERVER_FOLDER}/lib/
+cp ${PROJ_CLIENT_LIB} ${LIBCARLA_INSTALL_CLIENT_FOLDER}/lib/
 cp -p ${PROJ_SERVER_LIB} ${LIBCARLA_INSTALL_SERVER_FOLDER}/lib/
 
 # ==============================================================================
@@ -765,13 +825,13 @@ PATCHELF_VERSION=0.12
 PATCHELF_REPO=https://github.com/NixOS/patchelf/archive/${PATCHELF_VERSION}.tar.gz
 
 PATCHELF_TAR=${PATCHELF_VERSION}.tar.gz
-PATCHELF_SOURCE_DIR=patchelf-src
-PATCHELF_INSTALL_DIR=patchelf-install
+PATCHELF_SRC_DIR=patchelf-src
+PATCHELF_INSTALL_CLIENT_DIR=patchelf-client-install
 
-PATCHELF_INCLUDE_DIR=${PWD}/${PATCHELF_INSTALL_DIR}/include
-PATCHELF_EXE=${PWD}/${PATCHELF_INSTALL_DIR}/bin/patchelf
+PATCHELF_INCLUDE_DIR=${PWD}/${PATCHELF_INSTALL_CLIENT_DIR}/include
+PATCHELF_EXE=${PWD}/${PATCHELF_INSTALL_CLIENT_DIR}/bin/patchelf
 
-if [[ -d ${PATCHELF_INSTALL_DIR} ]] ; then
+if [[ -d ${PATCHELF_INSTALL_CLIENT_DIR} ]] ; then
   log "Patchelf already installed."
 else
   log "Retrieving patchelf"
@@ -787,24 +847,23 @@ else
   end=$(date +%s)
   echo "Elapsed Time Extracting patchelf: $(($end-$start)) seconds"
 
-  mv patchelf-${PATCHELF_VERSION} ${PATCHELF_SOURCE_DIR}
+  mv patchelf-${PATCHELF_VERSION} ${PATCHELF_SRC_DIR}
 
-  mkdir ${PATCHELF_INSTALL_DIR}
+  mkdir ${PATCHELF_INSTALL_CLIENT_DIR}
 
-  pushd ${PATCHELF_SOURCE_DIR} >/dev/null
+  pushd ${PATCHELF_SRC_DIR} >/dev/null
 
   ./bootstrap.sh
-  ./configure --prefix=${PWD}/../${PATCHELF_INSTALL_DIR}
+  ./configure --prefix=${PWD}/../${PATCHELF_INSTALL_CLIENT_DIR}
   make
   make install
 
   popd >/dev/null
 
   rm -Rf ${PATCHELF_TAR}
-  rm -Rf ${PATCHELF_SOURCE_DIR}
+  rm -Rf ${PATCHELF_SRC_DIR}
 fi
 
-mkdir -p ${LIBCARLA_INSTALL_CLIENT_FOLDER}/bin/
 cp ${PATCHELF_EXE} ${LIBCARLA_INSTALL_CLIENT_FOLDER}/bin/
 
 # ==============================================================================
@@ -815,7 +874,7 @@ if ${USE_PYTORCH} ; then
 
   LIBTORCH_BASENAME=libtorch
 
-  LIBTORCH_PATH=${PWD}/${LIBTORCH_BASENAME}
+  LIBTORCH_PATH=${PWD}/${LIBTORCH_BASENAME}-server-install
   LIBTORCH_INCLUDE=${LIBTORCH_PATH}/include
   LIBTORCH_LIB=${LIBTORCH_PATH}/lib
   LIBTORCH_ZIPFILE=libtorch-shared-with-deps-1.11.0+cu113.zip
@@ -846,6 +905,7 @@ if ${USE_PYTORCH} ; then
 
       cmake -DCMAKE_PREFIX_PATH="${LIBTORCH_PATH}" \
           -DCMAKE_CUDA_COMPILER="/usr/local/cuda/bin/nvcc" \
+          -DCMAKE_TOOLCHAIN_FILE="${CARLA_SERVER_TOOLCHAIN_FILE}" \
           -DCMAKE_INSTALL_PREFIX="${LIB_INSTALL}" \
           -DCMAKE_CUDA_FLAGS="-gencode=arch=compute_35,code=sm_35 -gencode=arch=compute_37,code=sm_37 -gencode=arch=compute_50,code=sm_50 -gencode=arch=compute_52,code=sm_52 -gencode=arch=compute_53,code=sm_53 -gencode=arch=compute_60,code=sm_60 -gencode=arch=compute_61,code=sm_61 -gencode=arch=compute_62,code=sm_62 -gencode=arch=compute_70,code=sm_70 -gencode=arch=compute_72,code=sm_72 -gencode=arch=compute_75,code=sm_75 -gencode=arch=compute_80,code=sm_80 -gencode=arch=compute_86,code=sm_86 -gencode=arch=compute_87,code=sm_87 -Wno-deprecated-gpu-targets" \
           -DWITH_CUDA=ON \
@@ -860,29 +920,28 @@ if ${USE_PYTORCH} ; then
   log "Build libtorch scatter"
   #LibtorchScatter
   LIBTORCHSCATTER_BASENAME=libtorchscatter
-  LIBTORCHSCATTER_SOURCE_DIR=${PWD}/${LIBTORCHSCATTER_BASENAME}-source
-  LIBTORCHSCATTER_INSTALL_DIR=${PWD}/${LIBTORCHSCATTER_BASENAME}-install
-  LIBTORCHSCATTER_INCLUDE=${LIBTORCHSCATTER_INSTALL_DIR}/include
-  LIBTORCHSCATTER_LIB=${LIBTORCHSCATTER_INSTALL_DIR}/lib
+  LIBTORCHSCATTER_SRC_DIR=${PWD}/${LIBTORCHSCATTER_BASENAME}-source
+  LIBTORCHSCATTER_INSTALL_SERVER_DIR=${PWD}/${LIBTORCHSCATTER_BASENAME}-server-install
+  LIBTORCHSCATTER_INCLUDE=${LIBTORCHSCATTER_INSTALL_SERVER_DIR}/include
+  LIBTORCHSCATTER_LIB=${LIBTORCHSCATTER_INSTALL_SERVER_DIR}/lib
   LIBTORCHSCATTER_REPO="https://github.com/rusty1s/pytorch_scatter.git"
 
-  build_torch_extension ${LIBTORCHSCATTER_SOURCE_DIR} ${LIBTORCHSCATTER_INSTALL_DIR} "${LIBTORCHSCATTER_REPO}"
+  build_torch_extension ${LIBTORCHSCATTER_SRC_DIR} ${LIBTORCHSCATTER_INSTALL_SERVER_DIR} "${LIBTORCHSCATTER_REPO}"
 
   log "Build libtorch cluster"
   LIBTORCHCLUSTER_BASENAME=libtorchcluster
-  LIBTORCHCLUSTER_SOURCE_DIR=${PWD}/${LIBTORCHCLUSTER_BASENAME}-source
-  LIBTORCHCLUSTER_INSTALL_DIR=${PWD}/${LIBTORCHCLUSTER_BASENAME}-install
-  LIBTORCHCLUSTER_INCLUDE=${LIBTORCHCLUSTER_INSTALL_DIR}/include
-  LIBTORCHCLUSTER_LIB=${LIBTORCHCLUSTER_INSTALL_DIR}/lib
+  LIBTORCHCLUSTER_SRC_DIR=${PWD}/${LIBTORCHCLUSTER_BASENAME}-source
+  LIBTORCHCLUSTER_INSTALL_SERVER_DIR=${PWD}/${LIBTORCHCLUSTER_BASENAME}-server-install
+  LIBTORCHCLUSTER_INCLUDE=${LIBTORCHCLUSTER_INSTALL_SERVER_DIR}/include
+  LIBTORCHCLUSTER_LIB=${LIBTORCHCLUSTER_INSTALL_SERVER_DIR}/lib
   LIBTORCHCLUSTER_REPO="https://github.com/rusty1s/pytorch_cluster.git"
 
-  build_torch_extension ${LIBTORCHCLUSTER_SOURCE_DIR} ${LIBTORCHCLUSTER_INSTALL_DIR} "${LIBTORCHCLUSTER_REPO}"
+  build_torch_extension ${LIBTORCHCLUSTER_SRC_DIR} ${LIBTORCHCLUSTER_INSTALL_SERVER_DIR} "${LIBTORCHCLUSTER_REPO}"
 
-  mkdir -p ${LIBCARLA_INSTALL_SERVER_FOLDER}/lib/
   cp -p ${LIBTORCH_LIB}/*.a ${LIBCARLA_INSTALL_SERVER_FOLDER}/lib/
   cp -p ${LIBTORCH_LIB}/*.so* ${LIBCARLA_INSTALL_SERVER_FOLDER}/lib/
-  cp -p ${LIBTORCHSCATTER_LIB}/*.so ${LIBCARLA_INSTALL_SERVER_FOLDER}/lib/
-  cp -p ${LIBTORCHCLUSTER_LIB}/*.so ${LIBCARLA_INSTALL_SERVER_FOLDER}/lib/
+  cp -p ${LIBTORCHSCATTER_LIB}/*.so* ${LIBCARLA_INSTALL_SERVER_FOLDER}/lib/
+  cp -p ${LIBTORCHCLUSTER_LIB}/*.so* ${LIBCARLA_INSTALL_SERVER_FOLDER}/lib/
 
   mkdir -p ${CARLAUE4_PLUGIN_ROOT_FOLDER}/Binaries/Linux/
   cp -p ${LIBTORCH_LIB}/*.so* ${CARLAUE4_PLUGIN_ROOT_FOLDER}/Binaries/Linux/
@@ -890,143 +949,108 @@ if ${USE_PYTORCH} ; then
   cp -p ${LIBTORCHCLUSTER_LIB}/*.so* ${CARLAUE4_PLUGIN_ROOT_FOLDER}/Binaries/Linux/
 fi
 
+
 # ==============================================================================
 # -- Download Fast DDS and dependencies ----------------------------------------
 # ==============================================================================
 
-FASTDDS_BASENAME=fast-dds
-FASTDDS_INSTALL_DIR=${PWD}/${FASTDDS_BASENAME}-install
-FASTDDS_INCLUDE=${FASTDDS_INSTALL_DIR}/include
-FASTDDS_LIB=${FASTDDS_INSTALL_DIR}/lib
+FASTDDS_BASENAME=${PWD}/fast-dds
+FASTDDS_SERVER_INCLUDE=${FASTDDS_BASENAME}-server-install/include
+FASTDDS_SERVER_LIB=${FASTDDS_BASENAME}-server-install/lib
 if ${USE_ROS2} ; then
-  function build_fastdds_extension {
-    LIB_SOURCE=$1
-    LIB_REPO=$2
-    LIB_BRANCH=$3
-    if [[ ! -d ${LIB_SOURCE} ]] ; then
-      mkdir -p ${LIB_SOURCE}
-      log "${LIB_REPO}"
-      start=$(date +%s)
-      git clone --depth 1 --branch ${LIB_BRANCH} ${LIB_REPO} ${LIB_SOURCE}
-      end=$(date +%s)
-      echo "Elapsed Time dowloading fastdds extension: $(($end-$start)) seconds"
-      mkdir -p ${LIB_SOURCE}/build
-    fi
-  }
-  if [[ ! -d ${FASTDDS_INSTALL_DIR} ]] ; then
-    mkdir -p ${FASTDDS_INSTALL_DIR}
-    log "Build foonathan memory vendor"
-    FOONATHAN_MEMORY_VENDOR_BASENAME=foonathan-memory-vendor
-    FOONATHAN_MEMORY_VENDOR_SOURCE_DIR=${PWD}/${FOONATHAN_MEMORY_VENDOR_BASENAME}-source
-    FOONATHAN_MEMORY_VENDOR_REPO="https://github.com/eProsima/foonathan_memory_vendor.git"
-    FOONATHAN_MEMORY_VENDOR_BRANCH=master
-    build_fastdds_extension ${FOONATHAN_MEMORY_VENDOR_SOURCE_DIR} "${FOONATHAN_MEMORY_VENDOR_REPO}" "${FOONATHAN_MEMORY_VENDOR_BRANCH}"
-    pushd ${FOONATHAN_MEMORY_VENDOR_SOURCE_DIR}/build >/dev/null
-    cmake -G "Ninja" \
-      -DCMAKE_INSTALL_PREFIX="${FASTDDS_INSTALL_DIR}" \
-      -DBUILD_SHARED_LIBS=ON \
-      -DCMAKE_CXX_FLAGS_RELEASE="-D_GLIBCXX_USE_CXX11_ABI=0" \
-      -DFOONATHAN_MEMORY_FORCE_VENDORED_BUILD=ON \
-      ..
-    ninja
-    ninja install
-    popd >/dev/null
-    rm -Rf ${FOONATHAN_MEMORY_VENDOR_SOURCE_DIR}
-
-    log "Build fast cdr"
-    FAST_CDR_BASENAME=fast-cdr
-    FAST_CDR_SOURCE_DIR=${PWD}/${FAST_CDR_BASENAME}-source
-    FAST_CDR_REPO="https://github.com/eProsima/Fast-CDR.git"
-    FAST_CDR_BRANCH=1.1.x
-    build_fastdds_extension ${FAST_CDR_SOURCE_DIR} "${FAST_CDR_REPO}" "${FAST_CDR_BRANCH}"
-    pushd ${FAST_CDR_SOURCE_DIR}/build >/dev/null
-    cmake -G "Ninja" \
-      -DCMAKE_INSTALL_PREFIX="${FASTDDS_INSTALL_DIR}" \
-      -DCMAKE_CXX_FLAGS_RELEASE="-D_GLIBCXX_USE_CXX11_ABI=0" \
-      ..
-    ninja
-    ninja install
-    popd >/dev/null
-    rm -Rf ${FAST_CDR_SOURCE_DIR}
+  if [[ -d ${FASTDDS_BASENAME}-server-install ]] ; then
+    log "FastDDS already installed."
+  else
+    rm -Rf \
+        ${FASTDDS_BASENAME}-source \
+        ${FASTDDS_BASENAME}-server-build \
+        ${FASTDDS_BASENAME}-server-install
+    mkdir -p ${FASTDDS_BASENAME}-server-install/lib
 
     log "Build fast dds"
-    FAST_DDS_LIB_BASENAME=fast-dds-lib
-    FAST_DDS_LIB_SOURCE_DIR=${PWD}/${FAST_DDS_LIB_BASENAME}-source
     FAST_DDS_LIB_REPO="https://github.com/eProsima/Fast-DDS.git"
-    FAST_DDS_LIB_BRANCH=2.11.2
-    build_fastdds_extension ${FAST_DDS_LIB_SOURCE_DIR} "${FAST_DDS_LIB_REPO}" "${FAST_DDS_LIB_BRANCH}"
-    pushd ${FAST_DDS_LIB_SOURCE_DIR}/build >/dev/null
+    FAST_DDS_LIB_BRANCH=2.11.3
+    # dependency foonath is not a submodule
+    FOONATHAN_MEMORY_VENDOR_REPO="https://github.com/eProsima/foonathan_memory_vendor.git"
+    FOONATHAN_MEMORY_VENDOR_BRANCH=master
+    FOONATHAN_MEMORY_VENDOR_SRC_DIR=${FASTDDS_BASENAME}-source/thirdparty/foonathan-memory-vendor
+
+    mkdir -p ${FASTDDS_BASENAME}-server-build
+
+    log "Prepare fast dds sources"
+    git clone --depth 1 --branch ${FAST_DDS_LIB_BRANCH} ${FAST_DDS_LIB_REPO} ${FASTDDS_BASENAME}-source
+    pushd ${FASTDDS_BASENAME}-source >/dev/null
+    git submodule update --init
+    git clone --depth 1 --branch ${FOONATHAN_MEMORY_VENDOR_BRANCH} ${FOONATHAN_MEMORY_VENDOR_REPO} ${FOONATHAN_MEMORY_VENDOR_SRC_DIR}
+
+    # we have to tweak the sources a bit to be able to compile with our boost version AND without exceptions
+    if [[ -e ${FASTDDS_BASENAME}-source/thirdparty/boost/include/boost ]]; then
+      # remove their boost includes, but keep their entry point
+      rm -rf ${FASTDDS_BASENAME}-source/thirdparty/boost/include/boost
+      # ensure the find boost compiles without exceptions
+      sed -i s/"CXX_STANDARD 11"/"CXX_STANDARD 11\n         COMPILE_DEFINITIONS \"-DBOOST_NO_EXCEPTIONS\""/ ${FASTDDS_BASENAME}-source/cmake/modules/FindThirdpartyBoost.cmake
+      sed -i s/"class ThirdpartyBoostCompileTest"/"#ifdef BOOST_NO_EXCEPTIONS\nnamespace boost {void throw_exception(std::exception const \& e) {}}\n#endif\nclass ThirdpartyBoostCompileTest"/ ${FASTDDS_BASENAME}-source/thirdparty/boost/test/ThirdpartyBoostCompile_test.cpp
+    fi
+    popd >/dev/null
+
+    log "fast dds: Build asio"
+    pushd ${FASTDDS_BASENAME}-source/thirdparty/asio/asio
+    ./autogen.sh
+    popd >/dev/null
+    mkdir -p ${FASTDDS_BASENAME}-server-build/asio
+    pushd ${FASTDDS_BASENAME}-server-build/asio >/dev/null
+    # since ASIO is header only installation, we don't care about flags from CARLA_SERVER_TOOLCHAIN_FILE
+    ${FASTDDS_BASENAME}-source/thirdparty/asio/asio/configure --prefix="${FASTDDS_BASENAME}-server-install"
+    make -j 15 install-data
+    popd >/dev/null
+
+    log "fast dds: Build foonathan memory vendor"
+    mkdir -p ${FASTDDS_BASENAME}-server-build/foonathan-memory-vendor
+    pushd ${FASTDDS_BASENAME}-server-build/foonathan-memory-vendor >/dev/null
     cmake -G "Ninja" \
-      -DCMAKE_INSTALL_PREFIX="${FASTDDS_INSTALL_DIR}" \
-      -DCMAKE_CXX_FLAGS=-latomic \
-      -DCMAKE_CXX_FLAGS_RELEASE="-D_GLIBCXX_USE_CXX11_ABI=0" \
-      -DTHIRDPARTY_Asio=FORCE \
-      -DTHIRDPARTY_TinyXML2=FORCE \
-      ..
+      -DCMAKE_INSTALL_PREFIX="${FASTDDS_BASENAME}-server-install" \
+      -DCMAKE_TOOLCHAIN_FILE="${CARLA_SERVER_TOOLCHAIN_FILE}" \
+      ${FASTDDS_BASENAME}-source/thirdparty/foonathan-memory-vendor
     ninja
     ninja install
     popd >/dev/null
-    rm -Rf ${FAST_DDS_LIB_SOURCE_DIR}
 
-    mkdir -p ${LIBCARLA_INSTALL_SERVER_FOLDER}/lib/
-    cp -p ${FASTDDS_LIB}/*.so* ${LIBCARLA_INSTALL_SERVER_FOLDER}/lib/
+    log "fast dds: Copy OpenSSL from UE4"
+    cp -r ${UE4_ROOT}/Engine/Source/ThirdParty/OpenSSL/1.1.1c/include/Linux/x86_64-unknown-linux-gnu/* ${FASTDDS_BASENAME}-server-install/include
+    cp -r ${UE4_ROOT}/Engine/Source/ThirdParty/OpenSSL/1.1.1c/lib/Linux/x86_64-unknown-linux-gnu/* ${FASTDDS_BASENAME}-server-install/lib
+
+    log "fast dds: Build fast dds itself"
+    mkdir -p ${FASTDDS_BASENAME}-server-build/fastdds
+    pushd ${FASTDDS_BASENAME}-server-build/fastdds >/dev/null
+
+    cmake -G "Ninja" \
+      -DCMAKE_INSTALL_PREFIX="${FASTDDS_BASENAME}-server-install" \
+      -DCMAKE_TOOLCHAIN_FILE="${CARLA_SERVER_TOOLCHAIN_FILE}" \
+      -DCMAKE_MODULE_PATH="${FASTDDS_BASENAME}-server-install" \
+      -DBUILD_TESTING=OFF \
+      -DCOMPILE_EXAMPLES=OFF \
+      -DCOMPILE_TOOLS=OFF \
+      -DTHIRDPARTY_Asio=FORCE \
+      -DTHIRDPARTY_fastcdr=FORCE \
+      -DTHIRDPARTY_TinyXML2=FORCE \
+      -DOPENSSL_FOUND:BOOL=ON \
+      -DOPENSSL_INCLUDE_DIR:FILEPATH=${FASTDDS_BASENAME}-server-install/include \
+      -DOPENSSL_SSL_LIBRARY:FILEPATH=${FASTDDS_BASENAME}-server-install/lib/libssl.a \
+      -DOPENSSL_CRYPTO_LIBRARY:FILEPATH=${FASTDDS_BASENAME}-server-install/lib/libcrypto.a \
+      -DCMAKE_CXX_FLAGS="-latomic" \
+      -DTHIRDPARTY_BOOST_INCLUDE_DIR="${BOOST_SERVER_INCLUDE};${FASTDDS_BASENAME}-source/thirdparty/boost/include" \
+      ${FASTDDS_BASENAME}-source
+    ninja
+    ninja install
+    popd >/dev/null
+
+    rm -Rf ${FASTDDS_BASENAME}-source ${FASTDDS_BASENAME}-server-build
   fi
+
+  cp -pr ${FASTDDS_SERVER_INCLUDE}/* ${LIBCARLA_INSTALL_SERVER_FOLDER}/include/
+  cp -p ${FASTDDS_SERVER_LIB}/*.a ${LIBCARLA_INSTALL_SERVER_FOLDER}/lib/
 fi
 
-# ==============================================================================
-# -- Generate Version.h --------------------------------------------------------
-# ==============================================================================
-
-CARLA_VERSION=$(get_git_repository_version)
-
-log "CARLA version ${CARLA_VERSION}."
-
-VERSION_H_FILE=${LIBCARLA_ROOT_FOLDER}/source/carla/Version.h
-VERSION_H_FILE_GEN=${CARLA_BUILD_FOLDER}/Version.h
-
-sed -e "s|\${CARLA_VERSION}|${CARLA_VERSION}|g" ${VERSION_H_FILE}.in > ${VERSION_H_FILE_GEN}
-
-move_if_changed "${VERSION_H_FILE_GEN}" "${VERSION_H_FILE}"
-
-# ==============================================================================
-# -- Generate CMake toolchains and config --------------------------------------
-# ==============================================================================
-
-log "Generating CMake configuration files."
-
-# -- LIBSTDCPP_TOOLCHAIN_FILE --------------------------------------------------
-
-cat >${LIBSTDCPP_TOOLCHAIN_FILE}.gen <<EOL
-# Automatically generated by `basename "$0"`
-
-set(CMAKE_C_COMPILER ${CC})
-set(CMAKE_CXX_COMPILER ${CXX})
-
-# disable -Werror since the boost 1.72 doesn't compile with ad_rss without warnings (i.e. the geometry headers)
-set(CMAKE_CXX_FLAGS "\${CMAKE_CXX_FLAGS} -std=c++14 -pthread -fPIC" CACHE STRING "" FORCE)
-set(CMAKE_CXX_FLAGS "\${CMAKE_CXX_FLAGS} -Wall -Wextra -Wpedantic" CACHE STRING "" FORCE)
-set(CMAKE_CXX_FLAGS "\${CMAKE_CXX_FLAGS} -Wdeprecated -Wshadow -Wuninitialized -Wunreachable-code" CACHE STRING "" FORCE)
-set(CMAKE_CXX_FLAGS "\${CMAKE_CXX_FLAGS} -Wpessimizing-move -Wold-style-cast -Wnull-dereference" CACHE STRING "" FORCE)
-set(CMAKE_CXX_FLAGS "\${CMAKE_CXX_FLAGS} -Wduplicate-enum -Wnon-virtual-dtor -Wheader-hygiene" CACHE STRING "" FORCE)
-set(CMAKE_CXX_FLAGS "\${CMAKE_CXX_FLAGS} -Wconversion -Wfloat-overflow-conversion" CACHE STRING "" FORCE)
-
-# @todo These flags need to be compatible with setup.py compilation.
-set(CMAKE_CXX_FLAGS_RELEASE_CLIENT "\${CMAKE_CXX_FLAGS_RELEASE} -DNDEBUG -g -fwrapv -O2 -Wall -Wstrict-prototypes -fno-strict-aliasing -Wdate-time -D_FORTIFY_SOURCE=2 -g -fstack-protector-strong -Wformat -Werror=format-security -fPIC -std=c++14 -Wno-missing-braces -DBOOST_ERROR_CODE_HEADER_ONLY" CACHE STRING "" FORCE)
-EOL
-
-# -- LIBCPP_TOOLCHAIN_FILE -----------------------------------------------------
-
-# We can reuse the previous toolchain.
-cp ${LIBSTDCPP_TOOLCHAIN_FILE}.gen ${LIBCPP_TOOLCHAIN_FILE}.gen
-
-cat >>${LIBCPP_TOOLCHAIN_FILE}.gen <<EOL
-
-set(CMAKE_CXX_FLAGS "\${CMAKE_CXX_FLAGS} -stdlib=libc++" CACHE STRING "" FORCE)
-set(CMAKE_CXX_FLAGS "\${CMAKE_CXX_FLAGS} -isystem ${LLVM_INCLUDE} ${UNREAL_HOSTED_CFLAGS}" CACHE STRING "" FORCE)
-set(CMAKE_CXX_FLAGS "\${CMAKE_CXX_FLAGS} -fno-exceptions" CACHE STRING "" FORCE)
-set(CMAKE_CXX_LINK_FLAGS "\${CMAKE_CXX_LINK_FLAGS} -L${LLVM_LIBPATH} ${UNREAL_HOSTED_CFLAGS}" CACHE STRING "" FORCE)
-set(CMAKE_CXX_LINK_FLAGS "\${CMAKE_CXX_LINK_FLAGS} -lc++ -lc++abi" CACHE STRING "" FORCE)
-EOL
 
 # -- CMAKE_CONFIG_FILE ---------------------------------------------------------
 
@@ -1050,36 +1074,38 @@ endif ()
 
 add_definitions(-DLIBCARLA_TEST_CONTENT_FOLDER="${LIBCARLA_TEST_CONTENT_FOLDER}")
 
-set(BOOST_INCLUDE_PATH "${BOOST_INCLUDE}")
-set(FASTDDS_INCLUDE_PATH "${FASTDDS_INCLUDE}")
-set(FASTDDS_LIB_PATH "${FASTDDS_LIB}")
 
 if (CMAKE_BUILD_TYPE STREQUAL "Server")
-  # Here libraries linking libc++.
-  set(LLVM_INCLUDE_PATH "${LLVM_INCLUDE}")
-  set(LLVM_LIB_PATH "${LLVM_LIBPATH}")
-  set(RPCLIB_INCLUDE_PATH "${RPCLIB_LIBCXX_INCLUDE}")
-  set(RPCLIB_LIB_PATH "${RPCLIB_LIBCXX_LIBPATH}")
-  set(GTEST_INCLUDE_PATH "${GTEST_LIBCXX_INCLUDE}")
-  set(GTEST_LIB_PATH "${GTEST_LIBCXX_LIBPATH}")
-elseif (CMAKE_BUILD_TYPE STREQUAL "ros2")
-  list(APPEND CMAKE_PREFIX_PATH "${FASTDDS_INSTALL_DIR}")
+  set(BOOST_INCLUDE_PATH "${BOOST_SERVER_INCLUDE}")
+  set(BOOST_LIB_PATH "${BOOST_SERVER_LIBPATH}")
+  set(ROS2_MW_INCLUDE_PATH "${FASTDDS_SERVER_INCLUDE}")
+  set(ROS2_MW_LIB_PATH "${FASTDDS_SERVER_LIB}")
+  set(ROS2_MW_LINK_LIBRARIES "fastrtps;fastcdr")
+  set(ROS2_MW_NAME "fastdds")
+  set(RPCLIB_INCLUDE_PATH "${RPCLIB_SERVER_INCLUDE}")
+  set(RPCLIB_LIB_PATH "${RPCLIB_SERVER_LIBPATH}")
+  set(GTEST_INCLUDE_PATH "${GTEST_SERVER_INCLUDE}")
+  set(GTEST_LIB_PATH "${GTEST_SERVER_LIBPATH}")
 elseif (CMAKE_BUILD_TYPE STREQUAL "Pytorch")
+  set(BOOST_INCLUDE_PATH "${BOOST_CLIENT_INCLUDE}")
+  set(BOOST_LIB_PATH "${BOOST_CLIENT_LIBPATH}")
   list(APPEND CMAKE_PREFIX_PATH "${LIBTORCH_PATH}")
-  list(APPEND CMAKE_PREFIX_PATH "${LIBTORCHSCATTER_INSTALL_DIR}")
-  list(APPEND CMAKE_PREFIX_PATH "${LIBTORCHCLUSTER_INSTALL_DIR}")
-  list(APPEND CMAKE_PREFIX_PATH "${LIBTORCHSPARSE_INSTALL_DIR}")
+  list(APPEND CMAKE_PREFIX_PATH "${LIBTORCHSCATTER_INSTALL_SERVER_DIR}")
+  list(APPEND CMAKE_PREFIX_PATH "${LIBTORCHCLUSTER_INSTALL_SERVER_DIR}")
+  list(APPEND CMAKE_PREFIX_PATH "${LIBTORCHSPARSE_INSTALL_CLIENT_DIR}")
 elseif (CMAKE_BUILD_TYPE STREQUAL "Client")
-  # Here libraries linking libstdc++.
-  set(RPCLIB_INCLUDE_PATH "${RPCLIB_LIBSTDCXX_INCLUDE}")
-  set(RPCLIB_LIB_PATH "${RPCLIB_LIBSTDCXX_LIBPATH}")
-  set(GTEST_INCLUDE_PATH "${GTEST_LIBSTDCXX_INCLUDE}")
-  set(GTEST_LIB_PATH "${GTEST_LIBSTDCXX_LIBPATH}")
-  set(BOOST_LIB_PATH "${BOOST_LIBPATH}")
-  set(RECAST_INCLUDE_PATH "${RECAST_INCLUDE}")
-  set(RECAST_LIB_PATH "${RECAST_LIBPATH}")
-  set(LIBPNG_INCLUDE_PATH "${LIBPNG_INCLUDE}")
-  set(LIBPNG_LIB_PATH "${LIBPNG_LIBPATH}")
+  set(BOOST_INCLUDE_PATH "${BOOST_CLIENT_INCLUDE}")
+  set(BOOST_LIB_PATH "${BOOST_CLIENT_LIBPATH}")
+  set(PROJ_INCLUDE_PATH "${PROJ_CLIENT_INCLUDE}")
+  set(PROJ_LIBPATH "${PROJ_CLIENT_LIBPATH}")
+  set(RPCLIB_INCLUDE_PATH "${RPCLIB_CLIENT_INCLUDE}")
+  set(RPCLIB_LIB_PATH "${RPCLIB_CLIENT_LIBPATH}")
+  set(GTEST_INCLUDE_PATH "${GTEST_CLIENT_INCLUDE}")
+  set(GTEST_LIB_PATH "${GTEST_CLIENT_LIBPATH}")
+  set(RECAST_INCLUDE_PATH "${RECAST_CLIENT_INCLUDE}")
+  set(RECAST_LIB_PATH "${RECAST_CLIENT_LIBPATH}")
+  set(LIBPNG_INCLUDE_PATH "${LIBPNG_CLIENT_INCLUDE}")
+  set(LIBPNG_LIB_PATH "${LIBPNG_CLIENT_LIBPATH}")
 endif ()
 
 EOL
@@ -1091,10 +1117,6 @@ else
   echo "add_definitions(-DLIBCARLA_IMAGE_WITH_PNG_SUPPORT=true)" >> ${CMAKE_CONFIG_FILE}.gen
 fi
 
-# -- Move files ----------------------------------------------------------------
-
-move_if_changed "${LIBSTDCPP_TOOLCHAIN_FILE}.gen" "${LIBSTDCPP_TOOLCHAIN_FILE}"
-move_if_changed "${LIBCPP_TOOLCHAIN_FILE}.gen" "${LIBCPP_TOOLCHAIN_FILE}"
 move_if_changed "${CMAKE_CONFIG_FILE}.gen" "${CMAKE_CONFIG_FILE}"
 
 # ==============================================================================
@@ -1102,5 +1124,4 @@ move_if_changed "${CMAKE_CONFIG_FILE}.gen" "${CMAKE_CONFIG_FILE}"
 # ==============================================================================
 
 popd >/dev/null
-
 log "Success!"
diff --git a/Util/BuildTools/Vars.mk b/Util/BuildTools/Vars.mk
index 3cfd55125..17b5b7d61 100644
--- a/Util/BuildTools/Vars.mk
+++ b/Util/BuildTools/Vars.mk
@@ -16,7 +16,6 @@ CARLA_PYTHONAPI_SOURCE_FOLDER=${CARLA_PYTHONAPI_ROOT_FOLDER}/carla
 LIBCARLA_ROOT_FOLDER=${CURDIR}/LibCarla
 LIBCARLA_BUILD_SERVER_FOLDER=${CARLA_BUILD_FOLDER}/libcarla-server-build
 LIBCARLA_BUILD_PYTORCH_FOLDER=${CARLA_BUILD_FOLDER}/libcarla-pytorch-build
-LIBCARLA_FASTDDS_FOLDER=${CARLA_BUILD_FOLDER}/libcarla-fastdds-install
 LIBCARLA_BUILD_CLIENT_FOLDER=${CARLA_BUILD_FOLDER}/libcarla-client-build
 LIBCARLA_INSTALL_SERVER_FOLDER=${CARLAUE4_PLUGIN_ROOT_FOLDER}/CarlaDependencies
 LIBCARLA_INSTALL_CLIENT_FOLDER=${CARLA_PYTHONAPI_SOURCE_FOLDER}/dependencies
@@ -27,8 +26,8 @@ OSM2ODR_SOURCE_FOLDER=${CARLA_BUILD_FOLDER}/libosm2dr-source
 
 CARLAUE4_PLUGIN_DEPS_FOLDER=${CARLAUE4_PLUGIN_ROOT_FOLDER}/CarlaDependencies
 
-LIBSTDCPP_TOOLCHAIN_FILE=${CARLA_BUILD_FOLDER}/LibStdCppToolChain.cmake
-LIBCPP_TOOLCHAIN_FILE=${CARLA_BUILD_FOLDER}/LibCppToolChain.cmake
+CARLA_CLIENT_TOOLCHAIN_FILE=${CARLA_BUILD_FOLDER}/CarlaClientToolChain.cmake
+CARLA_SERVER_TOOLCHAIN_FILE=${CARLA_BUILD_FOLDER}/CarlaServerToolChain.cmake
 CMAKE_CONFIG_FILE=${CARLA_BUILD_FOLDER}/CMakeLists.txt.in
 
 LIBCARLA_TEST_CONTENT_FOLDER=${CARLA_BUILD_FOLDER}/test-content
diff --git a/Util/BuildTools/Windows.mk b/Util/BuildTools/Windows.mk
index 1728f1a72..0a6fe1b7c 100644
--- a/Util/BuildTools/Windows.mk
+++ b/Util/BuildTools/Windows.mk
@@ -62,14 +62,14 @@ PythonAPI: LibCarla osm2odr
 	@"${CARLA_BUILD_TOOLS_FOLDER}/BuildPythonAPI.bat" --py3
 
 server: setup
-	@"${CARLA_BUILD_TOOLS_FOLDER}/BuildLibCarla.bat" --server --generator "$(GENERATOR)"
+	@"${CARLA_BUILD_TOOLS_FOLDER}/BuildLibCarla.bat" --server --generator "$(GENERATOR)" $(ARGS)
 
 client: setup
-	@"${CARLA_BUILD_TOOLS_FOLDER}/BuildLibCarla.bat" --client --generator "$(GENERATOR)"
+	@"${CARLA_BUILD_TOOLS_FOLDER}/BuildLibCarla.bat" --client --generator "$(GENERATOR)" $(ARGS)
 
 .PHONY: LibCarla
 LibCarla: setup
-	@"${CARLA_BUILD_TOOLS_FOLDER}/BuildLibCarla.bat" --server --client --generator "$(GENERATOR)"
+	@"${CARLA_BUILD_TOOLS_FOLDER}/BuildLibCarla.bat" --server --client --generator "$(GENERATOR)" $(ARGS)
 
 setup: downloadplugin
 	@"${CARLA_BUILD_TOOLS_FOLDER}/Setup.bat" --boost-toolset msvc-14.2 --generator "$(GENERATOR)" $(ARGS)
diff --git a/Util/Formatting/codeformat.py b/Util/Formatting/codeformat.py
index 11a753d17..4c173aafd 100755
--- a/Util/Formatting/codeformat.py
+++ b/Util/Formatting/codeformat.py
@@ -1,15 +1,14 @@
 #!/usr/bin/python
 
 #
-# Copyright (c) 2017-2020 Intel Corporation
+# Copyright (c) 2017-2024 Intel Corporation
 #
-# Helper script for code formatting using clang-format-3.9 and autopep
+# Helper script for code formatting using clang-format-14 and autopep
 
 import argparse
 import filecmp
 import os
 import re
-import sets
 import subprocess
 import sys
 from termcolor import cprint
@@ -31,9 +30,10 @@ class CodeFormatter:
 
     def verifyFormatterVersion(self):
         try:
-            versionOutput = subprocess.check_output([self.command, "--version"]).rstrip('\r\n')
+            versionOutputByteString = subprocess.check_output([self.command, "--version"])
+            versionOutput = versionOutputByteString.decode(encoding='UTF-8')
             if self.expectedVersion != "":
-                if versionOutput.startswith(self.expectedVersion):
+                if self.expectedVersion in versionOutput:
                     print("[OK] Found formatter '" + versionOutput + "'")
                     return
                 else:
@@ -41,8 +41,9 @@ class CodeFormatter:
                     cprint("[NOT OK] Version string does not start with '" + self.expectedVersion + "'", "red")
             else:
                 return
-        except:
+        except subprocess.CalledProcessError as exc:
             cprint("[ERROR] Could not run " + self.command, "red")
+            cprint("[ERROR] '" + exc.output + "'", "red")
             cprint("[INFO] Please check if correct version is installed or install with '" +
                    self.installCommand + "'", "blue")
         sys.exit(1)
@@ -70,9 +71,9 @@ class CodeFormatter:
         try:
             diffProcess = subprocess.Popen(
                 ["git", "diff", "--color=always", "--exit-code", "--no-index", "--", fileName, "-"],
-                                           stdin=subprocess.PIPE,
-                                           stdout=subprocess.PIPE,
-                                           stderr=subprocess.PIPE)
+                stdin=subprocess.PIPE,
+                stdout=subprocess.PIPE,
+                stderr=subprocess.PIPE)
             diffOutput, _ = diffProcess.communicate(verifyOutput)
             if diffProcess.returncode == 0:
                 diffOutput = ""
@@ -99,6 +100,8 @@ class CodeFormatter:
             if status:
                 return True
 
+        if not type(diffOutput) is str:
+            diffOutput = diffOutput.decode("utf-8")
         if diffOutput != "":
             cprint("[NOT OK] " + fileName, "red")
             if printDiff:
@@ -108,6 +111,9 @@ class CodeFormatter:
         print("[OK] " + fileName)
         return False
 
+    def setArgs(self, args):
+        self.args = args
+
 
 class CodeFormatterClang(CodeFormatter):
     CLANG_FORMAT_FILE = ".clang-format"
@@ -116,14 +122,14 @@ class CodeFormatterClang(CodeFormatter):
 
     def __init__(self):
         CodeFormatter.__init__(self,
-                               command="clang-format-3.9",
-                               expectedVersion="clang-format version 3.9",
+                               command="clang-format-14",
+                               expectedVersion="clang-format version 14",
                                formatCommandArguments=["-style=file", "-fallback-style=none", "-i"],
                                verifyCommandArguments=["-style=file", "-fallback-style=none"],
                                verifyOutputIsDiff=False,
                                fileEndings=["cpp", "hpp", "c", "h", "cc"],
                                fileDescription="source and header",
-                               installCommand="sudo apt-get install clang-format-3.9")
+                               installCommand="sudo apt-get install clang-format-14")
         self.scriptPath = os.path.dirname(os.path.abspath(__file__))
         self.checkedInClangFormatFile = os.path.join(self.scriptPath, CodeFormatterClang.CHECKED_IN_CLANG_FORMAT_FILE)
 
@@ -142,13 +148,13 @@ class CodeFormatterClang(CodeFormatter):
 
     def confirmWithUserClangFormatFileCantBeVerified(self):
         if not self.args.yes:
-            answer = raw_input("Are you sure your .clang-format file is up-to-date and you want to continue? (y/N)")
+            answer = input("Are you sure your .clang-format file is up-to-date and you want to continue? (y/N)")
             if answer != "y":
                 sys.exit(1)
 
     def verifyClangFormatFileExistsAndMatchesCheckedIn(self):
         self.verifyCheckedInClangFormatFileExists()
-        foundClangFormatFiles = sets.Set()
+        foundClangFormatFiles = set()
         for fileName in self.inputFiles:
             dirName = os.path.dirname(os.path.abspath(fileName))
             if not self.findClangFormatFileStartingFrom(dirName, fileName, foundClangFormatFiles):
@@ -219,6 +225,7 @@ class CodeFormat:
 
     def addCodeFormatter(self, codeFormatterInstance):
         self.codeFormatterInstances.append(codeFormatterInstance)
+        codeFormatterInstance.setArgs(self.args)
 
     def scanForInputFiles(self):
         for formatterInstance in self.codeFormatterInstances:
@@ -263,26 +270,26 @@ class CodeFormat:
 
     def confirmWithUserFileIsOutsideGit(self, fileName):
         cprint("[WARN] File is not in a Git repo: " + fileName, "yellow")
-        answer = raw_input("Are you sure to code format it anyway? (y/Q)")
+        answer = input("Are you sure to code format it anyway? (y/Q)")
         if answer != "y":
             sys.exit(1)
 
     def confirmWithUserFileIsUntracked(self, fileName):
         cprint("[WARN] File is untracked in Git: " + fileName, "yellow")
-        answer = raw_input("Are you sure to code format it anyway? (y/Q)")
+        answer = input("Are you sure to code format it anyway? (y/Q)")
         if answer != "y":
             sys.exit(1)
 
     def confirmWithUserGitRepoIsNotClean(self, gitRepo):
         cprint("[WARN] Git repo is not clean: " + gitRepo, "yellow")
-        answer = raw_input("Are you sure to code format files in it anyway? (y/Q)")
+        answer = input("Are you sure to code format files in it anyway? (y/Q)")
         if answer != "y":
             sys.exit(1)
 
     def checkInputFilesAreInCleanGitReposAndAreTracked(self):
         if self.args.verify or self.args.yes:
             return
-        gitRepos = sets.Set()
+        gitRepos = set()
         for formatterInstance in self.codeFormatterInstances:
             for fileName in formatterInstance.inputFiles:
                 gitRepo = self.getGitRepoForFile(fileName)
@@ -308,7 +315,7 @@ class CodeFormat:
                                           cwd=os.path.dirname(fileName))
             gitOutput, _ = gitProcess.communicate()
             if gitProcess.returncode == 0:
-                return gitOutput.rstrip('\r\n')
+                return gitOutput.decode(encoding='UTF-8').rstrip('\r\n')
         except OSError:
             cprint("[ERROR] Failed to run 'git rev-parse --show-toplevel' for " + fileName, "red")
         return None
@@ -322,7 +329,7 @@ class CodeFormat:
                                           cwd=os.path.dirname(fileName))
             gitOutput, _ = gitProcess.communicate()
             if gitProcess.returncode == 0:
-                return gitOutput.rstrip('\r\n') == "true"
+                return gitOutput.decode(encoding='UTF-8').rstrip('\r\n') == "true"
         except OSError:
             cprint("[ERROR] Failed to run 'git rev-parse --is-inside-work-tree' for " + fileName, "red")
         return False
@@ -400,13 +407,19 @@ class CodeFormat:
         elif (not self.args.verify) and (not self.args.yes) and self.numberOfInputFiles() > 1:
             for formatterInstance in self.codeFormatterInstances:
                 formatterInstance.printInputFiles()
-            answer = raw_input("Are you sure to code format " + str(self.numberOfInputFiles()) + " files? (y/N)")
+            answer = input("Are you sure to code format " + str(self.numberOfInputFiles()) + " files? (y/N)")
             if answer != "y":
                 sys.exit(1)
 
+    def checkPythonVersion(self):
+        if sys.version_info.major != 3:
+            cprint("[ERROR] Code Formatter runs in Python3 ", "red")
+            sys.exit(1)
+
 
 def main():
     codeFormat = CodeFormat()
+    codeFormat.checkPythonVersion()
     codeFormat.parseCommandLine()
     codeFormat.printMode()
 
@@ -425,5 +438,6 @@ def main():
         cprint("SUCCESS", "green")
         sys.exit(0)
 
+
 if __name__ == "__main__":
     main()
diff --git a/Util/InstallersWin/install_chrono.bat b/Util/InstallersWin/install_chrono.bat
index 3c701a7ca..19dec8602 100644
--- a/Util/InstallersWin/install_chrono.bat
+++ b/Util/InstallersWin/install_chrono.bat
@@ -14,6 +14,7 @@ echo %FILE_N% [Batch params]: %*
 rem ============================================================================
 rem -- Parse arguments ---------------------------------------------------------
 rem ============================================================================
+set GENERATOR=""
 
 :arg-parse
 if not "%1"=="" (
@@ -39,6 +40,11 @@ rem If not set set the build dir to the current dir
 if "%BUILD_DIR%" == "" set BUILD_DIR=%~dp0
 if not "%BUILD_DIR:~-1%"=="\" set BUILD_DIR=%BUILD_DIR%\
 if %GENERATOR% == "" set GENERATOR="Visual Studio 16 2019"
+echo.%GENERATOR% | findstr /C:"Visual Studio" >nul && (
+    set PLATFORM=-A x64
+) || (
+    set PLATFORM=
+)
 
 rem ============================================================================
 rem -- Get Eigen (Chrono dependency) -------------------------------------------
@@ -101,12 +107,6 @@ if not exist %CHRONO_INSTALL_DIR% (
 
     cd "%CHRONO_BUILD_DIR%"
 
-    echo.%GENERATOR% | findstr /C:"Visual Studio" >nul && (
-        set PLATFORM=-A x64
-    ) || (
-        set PLATFORM=
-    )
-
     echo %FILE_N% Compiling Chrono.
     cmake -G %GENERATOR% %PLATFORM%^
         -DCMAKE_BUILD_TYPE=Release^
diff --git a/Util/InstallersWin/install_eigen.bat b/Util/InstallersWin/install_eigen.bat
index 23448f74a..fc7b31a79 100644
--- a/Util/InstallersWin/install_eigen.bat
+++ b/Util/InstallersWin/install_eigen.bat
@@ -74,8 +74,18 @@ if not exist "%EIGEN_INSTALL_DIR%" (
 
 xcopy /q /Y /S /I /d "%EIGEN_SRC_DIR%\Eigen" "%EIGEN_INCLUDE%\Eigen"
 xcopy /q /Y /S /I /d "%EIGEN_SRC_DIR%\unsupported\Eigen" "%EIGEN_INCLUDE%\unsupported\Eigen"
-copy "%BUILD_DIR%..\Util\Patches\Eigen3.1.0\Macros.h" "%EIGEN_INCLUDE%\Eigen\src\Core\util\Macros.h"
+copy "%BUILD_DIR%..\Util\Patches\Eigen3.1.0\Functors.h" "%EIGEN_INCLUDE%\Eigen\src\Core\Functors.h"
 copy "%BUILD_DIR%..\Util\Patches\Eigen3.1.0\VectorBlock.h" "%EIGEN_INCLUDE%\Eigen\src\Core\VectorBlock.h"
+copy "%BUILD_DIR%..\Util\Patches\Eigen3.1.0\PacketMath.h" "%EIGEN_INCLUDE%\Eigen\src\Core\arch\SSE\PacketMath.h"
+copy "%BUILD_DIR%..\Util\Patches\Eigen3.1.0\SelfadjointMatrixVector.h" "%EIGEN_INCLUDE%\Eigen\src\Core\products\SelfadjointMatrixVector.h"
+copy "%BUILD_DIR%..\Util\Patches\Eigen3.1.0\Macros.h" "%EIGEN_INCLUDE%\Eigen\src\Core\util\Macros.h"
+copy "%BUILD_DIR%..\Util\Patches\Eigen3.1.0\ArrayCwiseUnaryOps.h" "%EIGEN_INCLUDE%\Eigen\src\plugins\ArrayCwiseUnaryOps.h"
+copy "%BUILD_DIR%..\Util\Patches\Eigen3.1.0\MatrixCwiseUnaryOps.h" "%EIGEN_INCLUDE%\Eigen\src\plugins\MatrixCwiseUnaryOps.h"
+
+  cp $CARLA_ROOT_FOLDER/Util/Patches/Eigen${EIGEN_VERSION}/Macros.h "${EIGEN_BASENAME}-source/Eigen/src/Core/util/Macros.h"
+  cp $CARLA_ROOT_FOLDER/Util/Patches/Eigen${EIGEN_VERSION}/ArrayCwiseUnaryOps.h "${EIGEN_BASENAME}-source/Eigen/src/plugins/ArrayCwiseUnaryOps.h"
+  cp $CARLA_ROOT_FOLDER/Util/Patches/Eigen${EIGEN_VERSION}/MatrixCwiseUnaryOps.h "${EIGEN_BASENAME}-source/Eigen/src/plugins/MatrixCwiseUnaryOps.h"
+
 
 goto success
 
diff --git a/Util/InstallersWin/install_fastDDS.bat b/Util/InstallersWin/install_fastDDS.bat
index 4bdc6a8bb..00dd82904 100644
--- a/Util/InstallersWin/install_fastDDS.bat
+++ b/Util/InstallersWin/install_fastDDS.bat
@@ -14,7 +14,11 @@ rem ============================================================================
 rem -- Parse arguments ---------------------------------------------------------
 rem ============================================================================
 
+
 set DEL_SRC=false
+set BOOST_VERSION="unknown"
+set FASTDDS_INSTALL_DIR=""
+set GENERATOR=""
 
 :arg-parse
 if not "%1"=="" (
@@ -27,20 +31,48 @@ if not "%1"=="" (
         set DEL_SRC=true
     )
 
+    if "%1"=="--boost-version" (
+        set BOOST_VERSION=%2
+        shift
+    )
+
+    if "%1"=="--install-dir" (
+        set FASTDDS_INSTALL_DIR=%2
+        shift
+    )
+
+    if "%1"=="--generator" (
+        set GENERATOR=%2
+        shift
+    )
+
     shift
     goto :arg-parse
 )
 
+if %GENERATOR% == "" set GENERATOR="Visual Studio 16 2019"
+echo.%GENERATOR% | findstr /C:"Visual Studio" >nul && (
+    set PLATFORM=-A x64
+) || (
+    set PLATFORM=
+)
+echo "%GENERATOR%" "%PLATFORM%"
+
 rem If not set set the build dir to the current dir
 if "%BUILD_DIR%" == "" set BUILD_DIR=%~dp0
 if not "%BUILD_DIR:~-1%"=="\" set BUILD_DIR=%BUILD_DIR%\
 
 set FASTDDS_SRC=fastDDS-src
 set FASTDDS_SRC_DIR=%BUILD_DIR%%FASTDDS_SRC%\
-set FASTDDS_INSTALL=fastDDS-install
-set FASTDDS_INSTALL_DIR=%BUILD_DIR%%FASTDDS_INSTALL%\
-set FASTDDS_BUILD_DIR=%FASTDDS_SRC_DIR%build\
+if "%FASTDDS_INSTALL_DIR%" == "" (
+    set FASTDDS_INSTALL=fastDDS-install
+    set FASTDDS_INSTALL_DIR=%BUILD_DIR%%FASTDDS_INSTALL%\
+)
+set FASTDDS_BUILD_DIR=%FASTDDS_SRC_DIR%build\fastdds
 set FASTDDS_BASENAME=%FASTDDS_SRC%
+set FOONATHAN_MEMORY_VENDOR_BASENAME=foonathan-memory-vendor
+set FOONATHAN_MEMORY_VENDOR_SOURCE_DIR=%FASTDDS_SRC_DIR%\thirdparty\foonathan-memory-vendor
+set FOONATHAN_MEMORY_VENDOR_BUILD_DIR=%FASTDDS_SRC_DIR%build\foonathan-memory-vendor
 
 if exist "%FASTDDS_INSTALL_DIR%" (
     goto already_build
@@ -49,86 +81,86 @@ if exist "%FASTDDS_INSTALL_DIR%" (
 if not exist "%FASTDDS_SRC_DIR%" (
     echo %FILE_N% Cloning "Fast-DDS"
 
-    call git clone https://github.com/eProsima/Fast-DDS.git "%FASTDDS_SRC_DIR:~0,-1%"
-    call git submodule init
-    call git submodule update
+    git clone --depth 1 --branch 2.11.3 https://github.com/eProsima/Fast-DDS.git "%FASTDDS_SRC_DIR:~0,-1%"
     if %errorlevel% neq 0 goto error_git
+    git submodule init
+    if %errorlevel% neq 0 goto error_git
+    git submodule update
+    if %errorlevel% neq 0 goto error_git
+    git clone --depth 1 --branch master https://github.com/eProsima/foonathan_memory_vendor.git "%FOONATHAN_MEMORY_VENDOR_SOURCE_DIR%"
 ) else (
     echo %FILE_N% Not cloning "Fast-DDS" because already exists a folder called "%FASTDDS_SRC%".
 )
 
-echo Compiling fastCDR dependency...
-
-if not exist "%FASTDDS_SRC_DIR%/thirdparty/fastcdr/build" (
-    echo %FILE_N% Creating "%FASTDDS_SRC_DIR%/thirdparty/fastcdr/build"
-    cd "%FASTDDS_SRC_DIR%/thirdparty/fastcdr"
-    mkdir build
-    cd ../../
+if not exist "%FOONATHAN_MEMORY_VENDOR_BUILD_DIR%" (
+    echo %FILE_N% Creating "%FOONATHAN_MEMORY_VENDOR_BUILD_DIR%"
+    mkdir "%FOONATHAN_MEMORY_VENDOR_BUILD_DIR%"
 )
+cd "%FOONATHAN_MEMORY_VENDOR_BUILD_DIR%"
 
-cd "%FASTDDS_SRC_DIR%/thirdparty/fastcdr/build"
-echo %FILE_N% Generating build...
-
-cmake .. -G "Visual Studio 16 2019" -A x64^
+echo %FILE_N% Generating build: foonathan memory vendor ...
+cmake -G %GENERATOR% %PLATFORM%^
     -DCMAKE_BUILD_TYPE=Release^
     -DCMAKE_CXX_FLAGS_RELEASE="/MD /MP"^
     -DCMAKE_INSTALL_PREFIX="%FASTDDS_INSTALL_DIR:\=/%"^
-    -DCMAKE_CXX_FLAGS=/D_SILENCE_TR1_NAMESPACE_DEPRECATION_WARNING
+    -DBUILD_STATIC_LIBS=ON^
+    -DBUILD_SHARED_LIBS=OFF^
+    -DCMAKE_CXX_FLAGS="/DBOOST_NO_EXCEPTIONS /DASIO_NO_EXCEPTIONS"^
+    "%FOONATHAN_MEMORY_VENDOR_SOURCE_DIR%"
 if %errorlevel%  neq 0 goto error_cmake
 
-echo %FILE_N% Building...
+echo %FILE_N% Building foonathan memory vendor... 
 cmake --build . --config Release --target install
 
-if errorlevel  neq 0 goto error_install
+if exist "%FASTDDS_SRC_DIR%\thirdparty\boost\include\boost" (
+    echo %FILE_N% Preparing fastdds boost ... 
+    @REM remove their boost includes, but keep their entry point
+    rd /s /q "%FASTDDS_SRC_DIR%\thirdparty\boost\include\boost"
+    @REM  ensure the find boost compiles without exceptions
+    sed -i s/"CXX_STANDARD 11"/"CXX_STANDARD 11\n         COMPILE_DEFINITIONS \"-DBOOST_NO_EXCEPTIONS\""/ "%FASTDDS_SRC_DIR%\cmake\modules\FindThirdpartyBoost.cmake"
+    sed -i s/"class ThirdpartyBoostCompileTest"/"#ifdef BOOST_NO_EXCEPTIONS\nnamespace boost {void throw_exception(std::exception const \& e) {}}\n#endif\nclass ThirdpartyBoostCompileTest"/ "%FASTDDS_SRC_DIR%\thirdparty\boost\test\ThirdpartyBoostCompile_test.cpp"
+)
 
-cd ../../..
-
-@REM echo Compiling asio dependency...
-
-@REM if not exist "%FASTDDS_SRC_DIR%/thirdparty/asio/asio/build" (
-@REM     echo %FILE_N% Creating "%FASTDDS_SRC_DIR%/thirdparty/asio/asio/build"
-@REM     cd "%FASTDDS_SRC_DIR%/thirdparty/asio/asio"
-@REM     mkdir build
-@REM     cd ../../
-@REM )
-
-@REM cd "%FASTDDS_SRC_DIR%/thirdparty/asio/asio/build"
-@REM echo %FILE_N% Generating build...
-
-@REM cmake .. -G "Visual Studio 16 2019" -A x64^
-@REM     -DCMAKE_BUILD_TYPE=Release^
-@REM     -DCMAKE_CXX_FLAGS_RELEASE="/MD /MP"^
-@REM     -DCMAKE_INSTALL_PREFIX="%FASTDDS_INSTALL_DIR:\=/%"^
-@REM     -DCMAKE_CXX_FLAGS=/D_SILENCE_TR1_NAMESPACE_DEPRECATION_WARNING
-@REM if %errorlevel%  neq 0 goto error_cmake
-
-@REM echo %FILE_N% Building...
-@REM cmake --build . --config Release --target install
-
-@REM if errorlevel  neq 0 goto error_install
-
-@REM cd ../../../..
+if exist "%FASTDDS_SRC_DIR%\src\cpp\utils\StringMatching.cpp" (
+    echo %FILE_N% Patching fastdds ... 
+    sed -i s/"defined(__cplusplus_winrt)"/"(1)"/ "%FASTDDS_SRC_DIR%\src\cpp\utils\StringMatching.cpp"
+    sed -i s/"replace_all(pattern"/"replace_all(path"/ "%FASTDDS_SRC_DIR%\src\cpp\utils\StringMatching.cpp"
+)
 
 if not exist "%FASTDDS_BUILD_DIR%" (
     echo %FILE_N% Creating "%FASTDDS_BUILD_DIR%"
     mkdir "%FASTDDS_BUILD_DIR%"
 )
-
 cd "%FASTDDS_BUILD_DIR%"
-echo %FILE_N% Generating build...
+echo %FILE_N% Generating build: fastdds ...
 
-cmake .. -G "Visual Studio 16 2019" -A x64^
+cmake -G %GENERATOR% %PLATFORM%^
     -DCMAKE_BUILD_TYPE=Release^
     -DCMAKE_CXX_FLAGS_RELEASE="/MD /MP"^
     -DCMAKE_INSTALL_PREFIX="%FASTDDS_INSTALL_DIR:\=/%"^
-    -DCMAKE_CXX_FLAGS=/D_SILENCE_TR1_NAMESPACE_DEPRECATION_WARNING^
+    -DCMAKE_MODULE_PATH="%FASTDDS_INSTALL_DIR:\=/%"^
+    -DBUILD_STATIC_LIBS=ON^
+    -DBUILD_SHARED_LIBS=OFF^
+    -DBUILD_TESTING=OFF^
+    -DCOMPILE_EXAMPLES=OFF^
+    -DCOMPILE_TOOLS=OFF^
+    -DTHIRDPARTY_Asio=FORCE^
+    -DTHIRDPARTY_fastcdr=FORCE^
+    -DTHIRDPARTY_TinyXML2=FORCE^
+    -DTHIRDPARTY_BOOST_INCLUDE_DIR="%BUILD_DIR%boost-%BOOST_VERSION%-install\include;%FASTDDS_SRC_DIR%\thirdparty\boost\include"^
+    -DCMAKE_CXX_FLAGS="/D_SILENCE_TR1_NAMESPACE_DEPRECATION_WARNING /DBOOST_NO_EXCEPTIONS /DASIO_NO_EXCEPTIONS"^
     "%FASTDDS_SRC_DIR%"
 if %errorlevel%  neq 0 goto error_cmake
 
-echo %FILE_N% Building...
+echo %FILE_N% Building fastdds...
 cmake --build . --config Release --target install
 
-if errorlevel  neq 0 goto error_install
+if %errorlevel%  neq 0 goto error_install
+
+rem copy asio header files as they are not copied automatically by the above build, but we need those for handling the asio-exceptions
+copy %FASTDDS_SRC_DIR%thirdparty\asio\asio\include\asio.hpp %INSTALLATION_DIR%fastDDS-install\include\ > NUL
+xcopy /Y /S /I  %FASTDDS_SRC_DIR%thirdparty\asio\asio\include\asio\* %INSTALLATION_DIR%fastDDS-install\include\asio\* > NUL
+if %errorlevel%  neq 0 goto error_install
 
 rem Remove the downloaded Fast-DDS source because is no more needed
 if %DEL_SRC% == true (
@@ -179,7 +211,7 @@ rem ============================================================================
 
 :good_exit
     echo %FILE_N% Exiting...
-    endlocal & set install_recast=%FASTDDS_INSTALL_DIR%
+    endlocal & set install_dds=%FASTDDS_INSTALL_DIR%
     exit /b 0
 
 :bad_exit
diff --git a/Util/InstallersWin/install_gtest.bat b/Util/InstallersWin/install_gtest.bat
index ae9fb7de3..24e7fc1da 100644
--- a/Util/InstallersWin/install_gtest.bat
+++ b/Util/InstallersWin/install_gtest.bat
@@ -16,6 +16,7 @@ rem -- Parse arguments ---------------------------------------------------------
 rem ============================================================================
 
 set DEL_SRC=false
+set GENERATOR=""
 
 :arg-parse
 if not "%1"=="" (
@@ -36,6 +37,11 @@ if not "%1"=="" (
 )
 
 if %GENERATOR% == "" set GENERATOR="Visual Studio 16 2019"
+echo.%GENERATOR% | findstr /C:"Visual Studio" >nul && (
+    set PLATFORM=-A x64
+) || (
+    set PLATFORM=
+)
 
 rem If not set set the build dir to the current dir
 if "%BUILD_DIR%" == "" set BUILD_DIR=%~dp0
@@ -69,12 +75,6 @@ if not exist "%GT_BUILD_DIR%" (
 cd "%GT_BUILD_DIR%"
 echo %FILE_N% Generating build...
 
-echo.%GENERATOR% | findstr /C:"Visual Studio" >nul && (
-    set PLATFORM=-A x64
-) || (
-    set PLATFORM=
-)
-
 cmake .. -G %GENERATOR% %PLATFORM%^
     -DCMAKE_BUILD_TYPE=Release^
     -DCMAKE_CXX_FLAGS_RELEASE="/MD /MP"^
diff --git a/Util/InstallersWin/install_proj.bat b/Util/InstallersWin/install_proj.bat
index a0c192ca2..6a6f2fb74 100644
--- a/Util/InstallersWin/install_proj.bat
+++ b/Util/InstallersWin/install_proj.bat
@@ -15,6 +15,8 @@ rem ============================================================================
 rem -- Parse arguments ---------------------------------------------------------
 rem ============================================================================
 
+set GENERATOR=""
+
 :arg-parse
 if not "%1"=="" (
     if "%1"=="--build-dir" (
@@ -36,6 +38,11 @@ if not "%1"=="" (
 )
 
 if %GENERATOR% == "" set GENERATOR="Visual Studio 16 2019"
+echo.%GENERATOR% | findstr /C:"Visual Studio" >nul && (
+    set PLATFORM=-A x64
+) || (
+    set PLATFORM=
+)
 
 rem If not set set the build dir to the current dir
 if "%BUILD_DIR%" == "" set BUILD_DIR=%~dp0
@@ -79,12 +86,6 @@ move %BUILD_DIR%%PROJ_BASE_NAME% %PROJ_SRC_DIR%
 mkdir %PROJ_BUILD_DIR%
 cd %PROJ_BUILD_DIR%
 
-echo.%GENERATOR% | findstr /C:"Visual Studio" >nul && (
-    set PLATFORM=-A x64
-) || (
-    set PLATFORM=
-)
-
 cmake .. -G %GENERATOR% %PLATFORM%^
     -DCMAKE_CXX_FLAGS_RELEASE="/MD /MP"^
     -DCMAKE_CXX_FLAGS="/MD /MP"^
diff --git a/Util/InstallersWin/install_recast.bat b/Util/InstallersWin/install_recast.bat
index 74257a91b..472bd7067 100644
--- a/Util/InstallersWin/install_recast.bat
+++ b/Util/InstallersWin/install_recast.bat
@@ -15,6 +15,7 @@ rem -- Parse arguments ---------------------------------------------------------
 rem ============================================================================
 
 set DEL_SRC=false
+set GENERATOR=""
 
 :arg-parse
 if not "%1"=="" (
@@ -35,6 +36,11 @@ if not "%1"=="" (
 )
 
 if %GENERATOR% == "" set GENERATOR="Visual Studio 16 2019"
+echo.%GENERATOR% | findstr /C:"Visual Studio" >nul && (
+    set PLATFORM=-A x64
+) || (
+    set PLATFORM=
+)
 
 rem If not set set the build dir to the current dir
 if "%BUILD_DIR%" == "" set BUILD_DIR=%~dp0
@@ -71,12 +77,6 @@ if not exist "%RECAST_BUILD_DIR%" (
 cd "%RECAST_BUILD_DIR%"
 echo %FILE_N% Generating build...
 
-echo.%GENERATOR% | findstr /C:"Visual Studio" >nul && (
-    set PLATFORM=-A x64
-) || (
-    set PLATFORM=
-)
-
 cmake .. -G %GENERATOR% %PLATFORM%^
     -DCMAKE_BUILD_TYPE=Release^
     -DCMAKE_CXX_FLAGS_RELEASE="/MD /MP"^
diff --git a/Util/InstallersWin/install_rpclib.bat b/Util/InstallersWin/install_rpclib.bat
index 0925872eb..050de488f 100644
--- a/Util/InstallersWin/install_rpclib.bat
+++ b/Util/InstallersWin/install_rpclib.bat
@@ -16,6 +16,7 @@ rem -- Parse arguments ---------------------------------------------------------
 rem ============================================================================
 
 set DEL_SRC=false
+set GENERATOR=""
 
 :arg-parse
 if not "%1"=="" (
diff --git a/Util/InstallersWin/install_xercesc.bat b/Util/InstallersWin/install_xercesc.bat
index a99bf1b47..8789be1c8 100644
--- a/Util/InstallersWin/install_xercesc.bat
+++ b/Util/InstallersWin/install_xercesc.bat
@@ -14,6 +14,7 @@ echo %FILE_N% [Batch params]: %*
 rem ============================================================================
 rem -- Parse arguments ---------------------------------------------------------
 rem ============================================================================
+set GENERATOR=""
 
 :arg-parse
 if not "%1"=="" (
@@ -36,6 +37,11 @@ if not "%1"=="" (
 )
 
 if %GENERATOR% == "" set GENERATOR="Visual Studio 16 2019"
+echo.%GENERATOR% | findstr /C:"Visual Studio" >nul && (
+    set PLATFORM=-A x64
+) || (
+    set PLATFORM=
+)
 
 rem If not set set the build dir to the current dir
 if "%BUILD_DIR%" == "" set BUILD_DIR=%~dp0
@@ -121,12 +127,6 @@ if not exist "%XERCESC_INSTALL_DIR%include" (
     mkdir "%XERCESC_INSTALL_DIR%include"
 )
 
-echo.%GENERATOR% | findstr /C:"Visual Studio" >nul && (
-    set PLATFORM=-A x64
-) || (
-    set PLATFORM=
-)
-
 cmake .. -G %GENERATOR% %PLATFORM%^
   -DCMAKE_INSTALL_PREFIX="%XERCESC_INSTALL_DIR:\=/%"^
   -DBUILD_SHARED_LIBS=OFF^
diff --git a/Util/Patches/Eigen3.1.0/ArrayCwiseUnaryOps.h b/Util/Patches/Eigen3.1.0/ArrayCwiseUnaryOps.h
new file mode 100644
index 000000000..c7dbcac94
--- /dev/null
+++ b/Util/Patches/Eigen3.1.0/ArrayCwiseUnaryOps.h
@@ -0,0 +1,205 @@
+
+
+/** \returns an expression of the coefficient-wise absolute value of \c *this
+  *
+  * Example: \include Cwise_abs.cpp
+  * Output: \verbinclude Cwise_abs.out
+  *
+  * \sa abs2()
+  */
+EIGEN_STRONG_INLINE const CwiseUnaryOp<internal::scalar_abs_op<Scalar>, const Derived>
+abs() const
+{
+  return derived();
+}
+
+/** \returns an expression of the coefficient-wise squared absolute value of \c *this
+  *
+  * Example: \include Cwise_abs2.cpp
+  * Output: \verbinclude Cwise_abs2.out
+  *
+  * \sa abs(), square()
+  */
+EIGEN_STRONG_INLINE const CwiseUnaryOp<internal::scalar_abs2_op<Scalar>, const Derived>
+abs2() const
+{
+  return derived();
+}
+
+/** \returns an expression of the coefficient-wise exponential of *this.
+  *
+  * Example: \include Cwise_exp.cpp
+  * Output: \verbinclude Cwise_exp.out
+  *
+  * \sa pow(), log(), sin(), cos()
+  */
+inline const CwiseUnaryOp<internal::scalar_exp_op<Scalar>, const Derived>
+exp() const
+{
+  return derived();
+}
+
+/** \returns an expression of the coefficient-wise logarithm of *this.
+  *
+  * Example: \include Cwise_log.cpp
+  * Output: \verbinclude Cwise_log.out
+  *
+  * \sa exp()
+  */
+inline const CwiseUnaryOp<internal::scalar_log_op<Scalar>, const Derived>
+log() const
+{
+  return derived();
+}
+
+/** \returns an expression of the coefficient-wise square root of *this.
+  *
+  * Example: \include Cwise_sqrt.cpp
+  * Output: \verbinclude Cwise_sqrt.out
+  *
+  * \sa pow(), square()
+  */
+inline const CwiseUnaryOp<internal::scalar_sqrt_op<Scalar>, const Derived>
+sqrt() const
+{
+  return derived();
+}
+
+/** \returns an expression of the coefficient-wise cosine of *this.
+  *
+  * Example: \include Cwise_cos.cpp
+  * Output: \verbinclude Cwise_cos.out
+  *
+  * \sa sin(), acos()
+  */
+inline const CwiseUnaryOp<internal::scalar_cos_op<Scalar>, const Derived>
+cos() const
+{
+  return derived();
+}
+
+
+/** \returns an expression of the coefficient-wise sine of *this.
+  *
+  * Example: \include Cwise_sin.cpp
+  * Output: \verbinclude Cwise_sin.out
+  *
+  * \sa cos(), asin()
+  */
+inline const CwiseUnaryOp<internal::scalar_sin_op<Scalar>, const Derived>
+sin() const
+{
+  return derived();
+}
+
+/** \returns an expression of the coefficient-wise arc cosine of *this.
+  *
+  * Example: \include Cwise_acos.cpp
+  * Output: \verbinclude Cwise_acos.out
+  *
+  * \sa cos(), asin()
+  */
+inline const CwiseUnaryOp<internal::scalar_acos_op<Scalar>, const Derived>
+acos() const
+{
+  return derived();
+}
+
+/** \returns an expression of the coefficient-wise arc sine of *this.
+  *
+  * Example: \include Cwise_asin.cpp
+  * Output: \verbinclude Cwise_asin.out
+  *
+  * \sa sin(), acos()
+  */
+inline const CwiseUnaryOp<internal::scalar_asin_op<Scalar>, const Derived>
+asin() const
+{
+  return derived();
+}
+
+/** \returns an expression of the coefficient-wise tan of *this.
+  *
+  * Example: \include Cwise_tan.cpp
+  * Output: \verbinclude Cwise_tan.out
+  *
+  * \sa cos(), sin()
+  */
+inline const CwiseUnaryOp<internal::scalar_tan_op<Scalar>, Derived>
+tan() const
+{
+  return derived();
+}
+
+
+/** \returns an expression of the coefficient-wise power of *this to the given exponent.
+  *
+  * Example: \include Cwise_pow.cpp
+  * Output: \verbinclude Cwise_pow.out
+  *
+  * \sa exp(), log()
+  */
+inline const CwiseUnaryOp<internal::scalar_pow_op<Scalar>, const Derived>
+pow(const Scalar& exponent) const
+{
+  return CwiseUnaryOp<internal::scalar_pow_op<Scalar>, const Derived>
+          (derived(), internal::scalar_pow_op<Scalar>(exponent));
+}
+
+
+/** \returns an expression of the coefficient-wise inverse of *this.
+  *
+  * Example: \include Cwise_inverse.cpp
+  * Output: \verbinclude Cwise_inverse.out
+  *
+  * \sa operator/(), operator*()
+  */
+inline const CwiseUnaryOp<internal::scalar_inverse_op<Scalar>, const Derived>
+inverse() const
+{
+  return derived();
+}
+
+/** \returns an expression of the coefficient-wise square of *this.
+  *
+  * Example: \include Cwise_square.cpp
+  * Output: \verbinclude Cwise_square.out
+  *
+  * \sa operator/(), operator*(), abs2()
+  */
+inline const CwiseUnaryOp<internal::scalar_square_op<Scalar>, const Derived>
+square() const
+{
+  return derived();
+}
+
+/** \returns an expression of the coefficient-wise cube of *this.
+  *
+  * Example: \include Cwise_cube.cpp
+  * Output: \verbinclude Cwise_cube.out
+  *
+  * \sa square(), pow()
+  */
+inline const CwiseUnaryOp<internal::scalar_cube_op<Scalar>, const Derived>
+cube() const
+{
+  return derived();
+}
+
+#if __cplusplus < 201703L
+
+#define EIGEN_MAKE_SCALAR_CWISE_UNARY_OP(METHOD_NAME,FUNCTOR) \
+  inline const CwiseUnaryOp<std::binder2nd<FUNCTOR<Scalar> >, const Derived> \
+  METHOD_NAME(const Scalar& s) const { \
+    return CwiseUnaryOp<std::binder2nd<FUNCTOR<Scalar> >, const Derived> \
+            (derived(), std::bind2nd(FUNCTOR<Scalar>(), s)); \
+  }
+
+EIGEN_MAKE_SCALAR_CWISE_UNARY_OP(operator==,  std::equal_to)
+EIGEN_MAKE_SCALAR_CWISE_UNARY_OP(operator!=,  std::not_equal_to)
+EIGEN_MAKE_SCALAR_CWISE_UNARY_OP(operator<,   std::less)
+EIGEN_MAKE_SCALAR_CWISE_UNARY_OP(operator<=,  std::less_equal)
+EIGEN_MAKE_SCALAR_CWISE_UNARY_OP(operator>,   std::greater)
+EIGEN_MAKE_SCALAR_CWISE_UNARY_OP(operator>=,  std::greater_equal)
+
+#endif
diff --git a/Util/Patches/Eigen3.1.0/Functors.h b/Util/Patches/Eigen3.1.0/Functors.h
new file mode 100644
index 000000000..d2df20517
--- /dev/null
+++ b/Util/Patches/Eigen3.1.0/Functors.h
@@ -0,0 +1,1006 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra.
+//
+// Copyright (C) 2008-2010 Gael Guennebaud <gael.guennebaud@inria.fr>
+//
+// Eigen is free software; you can redistribute it and/or
+// modify it under the terms of the GNU Lesser General Public
+// License as published by the Free Software Foundation; either
+// version 3 of the License, or (at your option) any later version.
+//
+// Alternatively, you can redistribute it and/or
+// modify it under the terms of the GNU General Public License as
+// published by the Free Software Foundation; either version 2 of
+// the License, or (at your option) any later version.
+//
+// Eigen is distributed in the hope that it will be useful, but WITHOUT ANY
+// WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
+// FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License or the
+// GNU General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public
+// License and a copy of the GNU General Public License along with
+// Eigen. If not, see <http://www.gnu.org/licenses/>.
+
+#ifndef EIGEN_FUNCTORS_H
+#define EIGEN_FUNCTORS_H
+
+namespace Eigen {
+
+namespace internal {
+
+// associative functors:
+
+/** \internal
+  * \brief Template functor to compute the sum of two scalars
+  *
+  * \sa class CwiseBinaryOp, MatrixBase::operator+, class VectorwiseOp, MatrixBase::sum()
+  */
+template<typename Scalar> struct scalar_sum_op {
+  EIGEN_EMPTY_STRUCT_CTOR(scalar_sum_op)
+  EIGEN_STRONG_INLINE const Scalar operator() (const Scalar& a, const Scalar& b) const { return a + b; }
+  template<typename Packet>
+  EIGEN_STRONG_INLINE const Packet packetOp(const Packet& a, const Packet& b) const
+  { return internal::padd(a,b); }
+  template<typename Packet>
+  EIGEN_STRONG_INLINE const Scalar predux(const Packet& a) const
+  { return internal::predux(a); }
+};
+template<typename Scalar>
+struct functor_traits<scalar_sum_op<Scalar> > {
+  enum {
+    Cost = NumTraits<Scalar>::AddCost,
+    PacketAccess = packet_traits<Scalar>::HasAdd
+  };
+};
+
+/** \internal
+  * \brief Template functor to compute the product of two scalars
+  *
+  * \sa class CwiseBinaryOp, Cwise::operator*(), class VectorwiseOp, MatrixBase::redux()
+  */
+template<typename LhsScalar,typename RhsScalar> struct scalar_product_op {
+  enum {
+    // TODO vectorize mixed product
+    Vectorizable = is_same<LhsScalar,RhsScalar>::value && packet_traits<LhsScalar>::HasMul && packet_traits<RhsScalar>::HasMul
+  };
+  typedef typename scalar_product_traits<LhsScalar,RhsScalar>::ReturnType result_type;
+  EIGEN_EMPTY_STRUCT_CTOR(scalar_product_op)
+  EIGEN_STRONG_INLINE const result_type operator() (const LhsScalar& a, const RhsScalar& b) const { return a * b; }
+  template<typename Packet>
+  EIGEN_STRONG_INLINE const Packet packetOp(const Packet& a, const Packet& b) const
+  { return internal::pmul(a,b); }
+  template<typename Packet>
+  EIGEN_STRONG_INLINE const result_type predux(const Packet& a) const
+  { return internal::predux_mul(a); }
+};
+template<typename LhsScalar,typename RhsScalar>
+struct functor_traits<scalar_product_op<LhsScalar,RhsScalar> > {
+  enum {
+    Cost = (NumTraits<LhsScalar>::MulCost + NumTraits<RhsScalar>::MulCost)/2, // rough estimate!
+    PacketAccess = scalar_product_op<LhsScalar,RhsScalar>::Vectorizable
+  };
+};
+
+/** \internal
+  * \brief Template functor to compute the conjugate product of two scalars
+  *
+  * This is a short cut for conj(x) * y which is needed for optimization purpose; in Eigen2 support mode, this becomes x * conj(y)
+  */
+template<typename LhsScalar,typename RhsScalar> struct scalar_conj_product_op {
+
+  enum {
+    Conj = NumTraits<LhsScalar>::IsComplex
+  };
+  
+  typedef typename scalar_product_traits<LhsScalar,RhsScalar>::ReturnType result_type;
+  
+  EIGEN_EMPTY_STRUCT_CTOR(scalar_conj_product_op)
+  EIGEN_STRONG_INLINE const result_type operator() (const LhsScalar& a, const RhsScalar& b) const
+  { return conj_helper<LhsScalar,RhsScalar,Conj,false>().pmul(a,b); }
+  
+  template<typename Packet>
+  EIGEN_STRONG_INLINE const Packet packetOp(const Packet& a, const Packet& b) const
+  { return conj_helper<Packet,Packet,Conj,false>().pmul(a,b); }
+};
+template<typename LhsScalar,typename RhsScalar>
+struct functor_traits<scalar_conj_product_op<LhsScalar,RhsScalar> > {
+  enum {
+    Cost = NumTraits<LhsScalar>::MulCost,
+    PacketAccess = internal::is_same<LhsScalar, RhsScalar>::value && packet_traits<LhsScalar>::HasMul
+  };
+};
+
+/** \internal
+  * \brief Template functor to compute the min of two scalars
+  *
+  * \sa class CwiseBinaryOp, MatrixBase::cwiseMin, class VectorwiseOp, MatrixBase::minCoeff()
+  */
+template<typename Scalar> struct scalar_min_op {
+  EIGEN_EMPTY_STRUCT_CTOR(scalar_min_op)
+  EIGEN_STRONG_INLINE const Scalar operator() (const Scalar& a, const Scalar& b) const { using std::min; return (min)(a, b); }
+  template<typename Packet>
+  EIGEN_STRONG_INLINE const Packet packetOp(const Packet& a, const Packet& b) const
+  { return internal::pmin(a,b); }
+  template<typename Packet>
+  EIGEN_STRONG_INLINE const Scalar predux(const Packet& a) const
+  { return internal::predux_min(a); }
+};
+template<typename Scalar>
+struct functor_traits<scalar_min_op<Scalar> > {
+  enum {
+    Cost = NumTraits<Scalar>::AddCost,
+    PacketAccess = packet_traits<Scalar>::HasMin
+  };
+};
+
+/** \internal
+  * \brief Template functor to compute the max of two scalars
+  *
+  * \sa class CwiseBinaryOp, MatrixBase::cwiseMax, class VectorwiseOp, MatrixBase::maxCoeff()
+  */
+template<typename Scalar> struct scalar_max_op {
+  EIGEN_EMPTY_STRUCT_CTOR(scalar_max_op)
+  EIGEN_STRONG_INLINE const Scalar operator() (const Scalar& a, const Scalar& b) const { using std::max; return (max)(a, b); }
+  template<typename Packet>
+  EIGEN_STRONG_INLINE const Packet packetOp(const Packet& a, const Packet& b) const
+  { return internal::pmax(a,b); }
+  template<typename Packet>
+  EIGEN_STRONG_INLINE const Scalar predux(const Packet& a) const
+  { return internal::predux_max(a); }
+};
+template<typename Scalar>
+struct functor_traits<scalar_max_op<Scalar> > {
+  enum {
+    Cost = NumTraits<Scalar>::AddCost,
+    PacketAccess = packet_traits<Scalar>::HasMax
+  };
+};
+
+/** \internal
+  * \brief Template functor to compute the hypot of two scalars
+  *
+  * \sa MatrixBase::stableNorm(), class Redux
+  */
+template<typename Scalar> struct scalar_hypot_op {
+  EIGEN_EMPTY_STRUCT_CTOR(scalar_hypot_op)
+//   typedef typename NumTraits<Scalar>::Real result_type;
+  EIGEN_STRONG_INLINE const Scalar operator() (const Scalar& _x, const Scalar& _y) const
+  {
+    using std::max;
+    using std::min;
+    Scalar p = (max)(_x, _y);
+    Scalar q = (min)(_x, _y);
+    Scalar qp = q/p;
+    return p * sqrt(Scalar(1) + qp*qp);
+  }
+};
+template<typename Scalar>
+struct functor_traits<scalar_hypot_op<Scalar> > {
+  enum { Cost = 5 * NumTraits<Scalar>::MulCost, PacketAccess=0 };
+};
+
+/** \internal
+  * \brief Template functor to compute the pow of two scalars
+  */
+template<typename Scalar, typename OtherScalar> struct scalar_binary_pow_op {
+  EIGEN_EMPTY_STRUCT_CTOR(scalar_binary_pow_op)
+  inline Scalar operator() (const Scalar& a, const OtherScalar& b) const { return internal::pow(a, b); }
+};
+template<typename Scalar, typename OtherScalar>
+struct functor_traits<scalar_binary_pow_op<Scalar,OtherScalar> > {
+  enum { Cost = 5 * NumTraits<Scalar>::MulCost, PacketAccess = false };
+};
+
+// other binary functors:
+
+/** \internal
+  * \brief Template functor to compute the difference of two scalars
+  *
+  * \sa class CwiseBinaryOp, MatrixBase::operator-
+  */
+template<typename Scalar> struct scalar_difference_op {
+  EIGEN_EMPTY_STRUCT_CTOR(scalar_difference_op)
+  EIGEN_STRONG_INLINE const Scalar operator() (const Scalar& a, const Scalar& b) const { return a - b; }
+  template<typename Packet>
+  EIGEN_STRONG_INLINE const Packet packetOp(const Packet& a, const Packet& b) const
+  { return internal::psub(a,b); }
+};
+template<typename Scalar>
+struct functor_traits<scalar_difference_op<Scalar> > {
+  enum {
+    Cost = NumTraits<Scalar>::AddCost,
+    PacketAccess = packet_traits<Scalar>::HasSub
+  };
+};
+
+/** \internal
+  * \brief Template functor to compute the quotient of two scalars
+  *
+  * \sa class CwiseBinaryOp, Cwise::operator/()
+  */
+template<typename Scalar> struct scalar_quotient_op {
+  EIGEN_EMPTY_STRUCT_CTOR(scalar_quotient_op)
+  EIGEN_STRONG_INLINE const Scalar operator() (const Scalar& a, const Scalar& b) const { return a / b; }
+  template<typename Packet>
+  EIGEN_STRONG_INLINE const Packet packetOp(const Packet& a, const Packet& b) const
+  { return internal::pdiv(a,b); }
+};
+template<typename Scalar>
+struct functor_traits<scalar_quotient_op<Scalar> > {
+  enum {
+    Cost = 2 * NumTraits<Scalar>::MulCost,
+    PacketAccess = packet_traits<Scalar>::HasDiv
+  };
+};
+
+/** \internal
+  * \brief Template functor to compute the and of two booleans
+  *
+  * \sa class CwiseBinaryOp, ArrayBase::operator&&
+  */
+struct scalar_boolean_and_op {
+  EIGEN_EMPTY_STRUCT_CTOR(scalar_boolean_and_op)
+  EIGEN_STRONG_INLINE bool operator() (const bool& a, const bool& b) const { return a && b; }
+};
+template<> struct functor_traits<scalar_boolean_and_op> {
+  enum {
+    Cost = NumTraits<bool>::AddCost,
+    PacketAccess = false
+  };
+};
+
+/** \internal
+  * \brief Template functor to compute the or of two booleans
+  *
+  * \sa class CwiseBinaryOp, ArrayBase::operator||
+  */
+struct scalar_boolean_or_op {
+  EIGEN_EMPTY_STRUCT_CTOR(scalar_boolean_or_op)
+  EIGEN_STRONG_INLINE bool operator() (const bool& a, const bool& b) const { return a || b; }
+};
+template<> struct functor_traits<scalar_boolean_or_op> {
+  enum {
+    Cost = NumTraits<bool>::AddCost,
+    PacketAccess = false
+  };
+};
+
+// unary functors:
+
+/** \internal
+  * \brief Template functor to compute the opposite of a scalar
+  *
+  * \sa class CwiseUnaryOp, MatrixBase::operator-
+  */
+template<typename Scalar> struct scalar_opposite_op {
+  EIGEN_EMPTY_STRUCT_CTOR(scalar_opposite_op)
+  EIGEN_STRONG_INLINE const Scalar operator() (const Scalar& a) const { return -a; }
+  template<typename Packet>
+  EIGEN_STRONG_INLINE const Packet packetOp(const Packet& a) const
+  { return internal::pnegate(a); }
+};
+template<typename Scalar>
+struct functor_traits<scalar_opposite_op<Scalar> >
+{ enum {
+    Cost = NumTraits<Scalar>::AddCost,
+    PacketAccess = packet_traits<Scalar>::HasNegate };
+};
+
+/** \internal
+  * \brief Template functor to compute the absolute value of a scalar
+  *
+  * \sa class CwiseUnaryOp, Cwise::abs
+  */
+template<typename Scalar> struct scalar_abs_op {
+  EIGEN_EMPTY_STRUCT_CTOR(scalar_abs_op)
+  typedef typename NumTraits<Scalar>::Real result_type;
+  EIGEN_STRONG_INLINE const result_type operator() (const Scalar& a) const { return internal::abs(a); }
+  template<typename Packet>
+  EIGEN_STRONG_INLINE const Packet packetOp(const Packet& a) const
+  { return internal::pabs(a); }
+};
+template<typename Scalar>
+struct functor_traits<scalar_abs_op<Scalar> >
+{
+  enum {
+    Cost = NumTraits<Scalar>::AddCost,
+    PacketAccess = packet_traits<Scalar>::HasAbs
+  };
+};
+
+/** \internal
+  * \brief Template functor to compute the squared absolute value of a scalar
+  *
+  * \sa class CwiseUnaryOp, Cwise::abs2
+  */
+template<typename Scalar> struct scalar_abs2_op {
+  EIGEN_EMPTY_STRUCT_CTOR(scalar_abs2_op)
+  typedef typename NumTraits<Scalar>::Real result_type;
+  EIGEN_STRONG_INLINE const result_type operator() (const Scalar& a) const { return internal::abs2(a); }
+  template<typename Packet>
+  EIGEN_STRONG_INLINE const Packet packetOp(const Packet& a) const
+  { return internal::pmul(a,a); }
+};
+template<typename Scalar>
+struct functor_traits<scalar_abs2_op<Scalar> >
+{ enum { Cost = NumTraits<Scalar>::MulCost, PacketAccess = packet_traits<Scalar>::HasAbs2 }; };
+
+/** \internal
+  * \brief Template functor to compute the conjugate of a complex value
+  *
+  * \sa class CwiseUnaryOp, MatrixBase::conjugate()
+  */
+template<typename Scalar> struct scalar_conjugate_op {
+  EIGEN_EMPTY_STRUCT_CTOR(scalar_conjugate_op)
+  EIGEN_STRONG_INLINE const Scalar operator() (const Scalar& a) const { return internal::conj(a); }
+  template<typename Packet>
+  EIGEN_STRONG_INLINE const Packet packetOp(const Packet& a) const { return internal::pconj(a); }
+};
+template<typename Scalar>
+struct functor_traits<scalar_conjugate_op<Scalar> >
+{
+  enum {
+    Cost = NumTraits<Scalar>::IsComplex ? NumTraits<Scalar>::AddCost : 0,
+    PacketAccess = packet_traits<Scalar>::HasConj
+  };
+};
+
+/** \internal
+  * \brief Template functor to cast a scalar to another type
+  *
+  * \sa class CwiseUnaryOp, MatrixBase::cast()
+  */
+template<typename Scalar, typename NewType>
+struct scalar_cast_op {
+  EIGEN_EMPTY_STRUCT_CTOR(scalar_cast_op)
+  typedef NewType result_type;
+  EIGEN_STRONG_INLINE const NewType operator() (const Scalar& a) const { return cast<Scalar, NewType>(a); }
+};
+template<typename Scalar, typename NewType>
+struct functor_traits<scalar_cast_op<Scalar,NewType> >
+{ enum { Cost = is_same<Scalar, NewType>::value ? 0 : NumTraits<NewType>::AddCost, PacketAccess = false }; };
+
+/** \internal
+  * \brief Template functor to extract the real part of a complex
+  *
+  * \sa class CwiseUnaryOp, MatrixBase::real()
+  */
+template<typename Scalar>
+struct scalar_real_op {
+  EIGEN_EMPTY_STRUCT_CTOR(scalar_real_op)
+  typedef typename NumTraits<Scalar>::Real result_type;
+  EIGEN_STRONG_INLINE result_type operator() (const Scalar& a) const { return internal::real(a); }
+};
+template<typename Scalar>
+struct functor_traits<scalar_real_op<Scalar> >
+{ enum { Cost = 0, PacketAccess = false }; };
+
+/** \internal
+  * \brief Template functor to extract the imaginary part of a complex
+  *
+  * \sa class CwiseUnaryOp, MatrixBase::imag()
+  */
+template<typename Scalar>
+struct scalar_imag_op {
+  EIGEN_EMPTY_STRUCT_CTOR(scalar_imag_op)
+  typedef typename NumTraits<Scalar>::Real result_type;
+  EIGEN_STRONG_INLINE result_type operator() (const Scalar& a) const { return internal::imag(a); }
+};
+template<typename Scalar>
+struct functor_traits<scalar_imag_op<Scalar> >
+{ enum { Cost = 0, PacketAccess = false }; };
+
+/** \internal
+  * \brief Template functor to extract the real part of a complex as a reference
+  *
+  * \sa class CwiseUnaryOp, MatrixBase::real()
+  */
+template<typename Scalar>
+struct scalar_real_ref_op {
+  EIGEN_EMPTY_STRUCT_CTOR(scalar_real_ref_op)
+  typedef typename NumTraits<Scalar>::Real result_type;
+  EIGEN_STRONG_INLINE result_type& operator() (const Scalar& a) const { return internal::real_ref(*const_cast<Scalar*>(&a)); }
+};
+template<typename Scalar>
+struct functor_traits<scalar_real_ref_op<Scalar> >
+{ enum { Cost = 0, PacketAccess = false }; };
+
+/** \internal
+  * \brief Template functor to extract the imaginary part of a complex as a reference
+  *
+  * \sa class CwiseUnaryOp, MatrixBase::imag()
+  */
+template<typename Scalar>
+struct scalar_imag_ref_op {
+  EIGEN_EMPTY_STRUCT_CTOR(scalar_imag_ref_op)
+  typedef typename NumTraits<Scalar>::Real result_type;
+  EIGEN_STRONG_INLINE result_type& operator() (const Scalar& a) const { return internal::imag_ref(*const_cast<Scalar*>(&a)); }
+};
+template<typename Scalar>
+struct functor_traits<scalar_imag_ref_op<Scalar> >
+{ enum { Cost = 0, PacketAccess = false }; };
+
+/** \internal
+  *
+  * \brief Template functor to compute the exponential of a scalar
+  *
+  * \sa class CwiseUnaryOp, Cwise::exp()
+  */
+template<typename Scalar> struct scalar_exp_op {
+  EIGEN_EMPTY_STRUCT_CTOR(scalar_exp_op)
+  inline const Scalar operator() (const Scalar& a) const { return internal::exp(a); }
+  typedef typename packet_traits<Scalar>::type Packet;
+  inline Packet packetOp(const Packet& a) const { return internal::pexp(a); }
+};
+template<typename Scalar>
+struct functor_traits<scalar_exp_op<Scalar> >
+{ enum { Cost = 5 * NumTraits<Scalar>::MulCost, PacketAccess = packet_traits<Scalar>::HasExp }; };
+
+/** \internal
+  *
+  * \brief Template functor to compute the logarithm of a scalar
+  *
+  * \sa class CwiseUnaryOp, Cwise::log()
+  */
+template<typename Scalar> struct scalar_log_op {
+  EIGEN_EMPTY_STRUCT_CTOR(scalar_log_op)
+  inline const Scalar operator() (const Scalar& a) const { return internal::log(a); }
+  typedef typename packet_traits<Scalar>::type Packet;
+  inline Packet packetOp(const Packet& a) const { return internal::plog(a); }
+};
+template<typename Scalar>
+struct functor_traits<scalar_log_op<Scalar> >
+{ enum { Cost = 5 * NumTraits<Scalar>::MulCost, PacketAccess = packet_traits<Scalar>::HasLog }; };
+
+/** \internal
+  * \brief Template functor to multiply a scalar by a fixed other one
+  *
+  * \sa class CwiseUnaryOp, MatrixBase::operator*, MatrixBase::operator/
+  */
+/* NOTE why doing the pset1() in packetOp *is* an optimization ?
+ * indeed it seems better to declare m_other as a Packet and do the pset1() once
+ * in the constructor. However, in practice:
+ *  - GCC does not like m_other as a Packet and generate a load every time it needs it
+ *  - on the other hand GCC is able to moves the pset1() away the loop :)
+ *  - simpler code ;)
+ * (ICC and gcc 4.4 seems to perform well in both cases, the issue is visible with y = a*x + b*y)
+ */
+template<typename Scalar>
+struct scalar_multiple_op {
+  typedef typename packet_traits<Scalar>::type Packet;
+  // FIXME default copy constructors seems bugged with std::complex<>
+  EIGEN_STRONG_INLINE scalar_multiple_op(const scalar_multiple_op& other) : m_other(other.m_other) { }
+  EIGEN_STRONG_INLINE scalar_multiple_op(const Scalar& other) : m_other(other) { }
+  EIGEN_STRONG_INLINE Scalar operator() (const Scalar& a) const { return a * m_other; }
+  EIGEN_STRONG_INLINE const Packet packetOp(const Packet& a) const
+  { return internal::pmul(a, pset1<Packet>(m_other)); }
+  typename add_const_on_value_type<typename NumTraits<Scalar>::Nested>::type m_other;
+};
+template<typename Scalar>
+struct functor_traits<scalar_multiple_op<Scalar> >
+{ enum { Cost = NumTraits<Scalar>::MulCost, PacketAccess = packet_traits<Scalar>::HasMul }; };
+
+template<typename Scalar1, typename Scalar2>
+struct scalar_multiple2_op {
+  typedef typename scalar_product_traits<Scalar1,Scalar2>::ReturnType result_type;
+  EIGEN_STRONG_INLINE scalar_multiple2_op(const scalar_multiple2_op& other) : m_other(other.m_other) { }
+  EIGEN_STRONG_INLINE scalar_multiple2_op(const Scalar2& other) : m_other(other) { }
+  EIGEN_STRONG_INLINE result_type operator() (const Scalar1& a) const { return a * m_other; }
+  typename add_const_on_value_type<typename NumTraits<Scalar2>::Nested>::type m_other;
+};
+template<typename Scalar1,typename Scalar2>
+struct functor_traits<scalar_multiple2_op<Scalar1,Scalar2> >
+{ enum { Cost = NumTraits<Scalar1>::MulCost, PacketAccess = false }; };
+
+template<typename Scalar, bool IsInteger>
+struct scalar_quotient1_impl {
+  typedef typename packet_traits<Scalar>::type Packet;
+  // FIXME default copy constructors seems bugged with std::complex<>
+  EIGEN_STRONG_INLINE scalar_quotient1_impl(const scalar_quotient1_impl& other) : m_other(other.m_other) { }
+  EIGEN_STRONG_INLINE scalar_quotient1_impl(const Scalar& other) : m_other(static_cast<Scalar>(1) / other) {}
+  EIGEN_STRONG_INLINE Scalar operator() (const Scalar& a) const { return a * m_other; }
+  EIGEN_STRONG_INLINE const Packet packetOp(const Packet& a) const
+  { return internal::pmul(a, pset1<Packet>(m_other)); }
+  const Scalar m_other;
+};
+template<typename Scalar>
+struct functor_traits<scalar_quotient1_impl<Scalar,false> >
+{ enum { Cost = NumTraits<Scalar>::MulCost, PacketAccess = packet_traits<Scalar>::HasMul }; };
+
+template<typename Scalar>
+struct scalar_quotient1_impl<Scalar,true> {
+  // FIXME default copy constructors seems bugged with std::complex<>
+  EIGEN_STRONG_INLINE scalar_quotient1_impl(const scalar_quotient1_impl& other) : m_other(other.m_other) { }
+  EIGEN_STRONG_INLINE scalar_quotient1_impl(const Scalar& other) : m_other(other) {}
+  EIGEN_STRONG_INLINE Scalar operator() (const Scalar& a) const { return a / m_other; }
+  typename add_const_on_value_type<typename NumTraits<Scalar>::Nested>::type m_other;
+};
+template<typename Scalar>
+struct functor_traits<scalar_quotient1_impl<Scalar,true> >
+{ enum { Cost = 2 * NumTraits<Scalar>::MulCost, PacketAccess = false }; };
+
+/** \internal
+  * \brief Template functor to divide a scalar by a fixed other one
+  *
+  * This functor is used to implement the quotient of a matrix by
+  * a scalar where the scalar type is not necessarily a floating point type.
+  *
+  * \sa class CwiseUnaryOp, MatrixBase::operator/
+  */
+template<typename Scalar>
+struct scalar_quotient1_op : scalar_quotient1_impl<Scalar, NumTraits<Scalar>::IsInteger > {
+  EIGEN_STRONG_INLINE scalar_quotient1_op(const Scalar& other)
+    : scalar_quotient1_impl<Scalar, NumTraits<Scalar>::IsInteger >(other) {}
+};
+template<typename Scalar>
+struct functor_traits<scalar_quotient1_op<Scalar> >
+: functor_traits<scalar_quotient1_impl<Scalar, NumTraits<Scalar>::IsInteger> >
+{};
+
+// nullary functors
+
+template<typename Scalar>
+struct scalar_constant_op {
+  typedef typename packet_traits<Scalar>::type Packet;
+  EIGEN_STRONG_INLINE scalar_constant_op(const scalar_constant_op& other) : m_other(other.m_other) { }
+  EIGEN_STRONG_INLINE scalar_constant_op(const Scalar& other) : m_other(other) { }
+  template<typename Index>
+  EIGEN_STRONG_INLINE const Scalar operator() (Index, Index = 0) const { return m_other; }
+  template<typename Index>
+  EIGEN_STRONG_INLINE const Packet packetOp(Index, Index = 0) const { return internal::pset1<Packet>(m_other); }
+  const Scalar m_other;
+};
+template<typename Scalar>
+struct functor_traits<scalar_constant_op<Scalar> >
+// FIXME replace this packet test by a safe one
+{ enum { Cost = 1, PacketAccess = packet_traits<Scalar>::Vectorizable, IsRepeatable = true }; };
+
+template<typename Scalar> struct scalar_identity_op {
+  EIGEN_EMPTY_STRUCT_CTOR(scalar_identity_op)
+  template<typename Index>
+  EIGEN_STRONG_INLINE const Scalar operator() (Index row, Index col) const { return row==col ? Scalar(1) : Scalar(0); }
+};
+template<typename Scalar>
+struct functor_traits<scalar_identity_op<Scalar> >
+{ enum { Cost = NumTraits<Scalar>::AddCost, PacketAccess = false, IsRepeatable = true }; };
+
+template <typename Scalar, bool RandomAccess> struct linspaced_op_impl;
+
+// linear access for packet ops:
+// 1) initialization
+//   base = [low, ..., low] + ([step, ..., step] * [-size, ..., 0])
+// 2) each step
+//   base += [size*step, ..., size*step]
+template <typename Scalar>
+struct linspaced_op_impl<Scalar,false>
+{
+  typedef typename packet_traits<Scalar>::type Packet;
+
+  linspaced_op_impl(Scalar low, Scalar step) :
+  m_low(low), m_step(step),
+  m_packetStep(pset1<Packet>(packet_traits<Scalar>::size*step)),
+  m_base(padd(pset1<Packet>(low),pmul(pset1<Packet>(step),plset<Scalar>(-packet_traits<Scalar>::size)))) {}
+
+  template<typename Index>
+  EIGEN_STRONG_INLINE const Scalar operator() (Index i) const { return m_low+i*m_step; }
+  template<typename Index>
+  EIGEN_STRONG_INLINE const Packet packetOp(Index) const { return m_base = padd(m_base,m_packetStep); }
+
+  const Scalar m_low;
+  const Scalar m_step;
+  const Packet m_packetStep;
+  mutable Packet m_base;
+};
+
+// random access for packet ops:
+// 1) each step
+//   [low, ..., low] + ( [step, ..., step] * ( [i, ..., i] + [0, ..., size] ) )
+template <typename Scalar>
+struct linspaced_op_impl<Scalar,true>
+{
+  typedef typename packet_traits<Scalar>::type Packet;
+
+  linspaced_op_impl(Scalar low, Scalar step) :
+  m_low(low), m_step(step),
+  m_lowPacket(pset1<Packet>(m_low)), m_stepPacket(pset1<Packet>(m_step)), m_interPacket(plset<Scalar>(0)) {}
+
+  template<typename Index>
+  EIGEN_STRONG_INLINE const Scalar operator() (Index i) const { return m_low+i*m_step; }
+
+  template<typename Index>
+  EIGEN_STRONG_INLINE const Packet packetOp(Index i) const
+  { return internal::padd(m_lowPacket, pmul(m_stepPacket, padd(pset1<Packet>(i),m_interPacket))); }
+
+  const Scalar m_low;
+  const Scalar m_step;
+  const Packet m_lowPacket;
+  const Packet m_stepPacket;
+  const Packet m_interPacket;
+};
+
+// ----- Linspace functor ----------------------------------------------------------------
+
+// Forward declaration (we default to random access which does not really give
+// us a speed gain when using packet access but it allows to use the functor in
+// nested expressions).
+template <typename Scalar, bool RandomAccess = true> struct linspaced_op;
+template <typename Scalar, bool RandomAccess> struct functor_traits< linspaced_op<Scalar,RandomAccess> >
+{ enum { Cost = 1, PacketAccess = packet_traits<Scalar>::HasSetLinear, IsRepeatable = true }; };
+template <typename Scalar, bool RandomAccess> struct linspaced_op
+{
+  typedef typename packet_traits<Scalar>::type Packet;
+  linspaced_op(Scalar low, Scalar high, int num_steps) : impl((num_steps==1 ? high : low), (num_steps==1 ? Scalar() : (high-low)/(num_steps-1))) {}
+
+  template<typename Index>
+  EIGEN_STRONG_INLINE const Scalar operator() (Index i) const { return impl(i); }
+
+  // We need this function when assigning e.g. a RowVectorXd to a MatrixXd since
+  // there row==0 and col is used for the actual iteration.
+  template<typename Index>
+  EIGEN_STRONG_INLINE const Scalar operator() (Index row, Index col) const 
+  {
+    eigen_assert(col==0 || row==0);
+    return impl(col + row);
+  }
+
+  template<typename Index>
+  EIGEN_STRONG_INLINE const Packet packetOp(Index i) const { return impl.packetOp(i); }
+
+  // We need this function when assigning e.g. a RowVectorXd to a MatrixXd since
+  // there row==0 and col is used for the actual iteration.
+  template<typename Index>
+  EIGEN_STRONG_INLINE const Packet packetOp(Index row, Index col) const
+  {
+    eigen_assert(col==0 || row==0);
+    return impl.packetOp(col + row);
+  }
+
+  // This proxy object handles the actual required temporaries, the different
+  // implementations (random vs. sequential access) as well as the
+  // correct piping to size 2/4 packet operations.
+  const linspaced_op_impl<Scalar,RandomAccess> impl;
+};
+
+// all functors allow linear access, except scalar_identity_op. So we fix here a quick meta
+// to indicate whether a functor allows linear access, just always answering 'yes' except for
+// scalar_identity_op.
+// FIXME move this to functor_traits adding a functor_default
+template<typename Functor> struct functor_has_linear_access { enum { ret = 1 }; };
+template<typename Scalar> struct functor_has_linear_access<scalar_identity_op<Scalar> > { enum { ret = 0 }; };
+
+// in CwiseBinaryOp, we require the Lhs and Rhs to have the same scalar type, except for multiplication
+// where we only require them to have the same _real_ scalar type so one may multiply, say, float by complex<float>.
+// FIXME move this to functor_traits adding a functor_default
+template<typename Functor> struct functor_allows_mixing_real_and_complex { enum { ret = 0 }; };
+template<typename LhsScalar,typename RhsScalar> struct functor_allows_mixing_real_and_complex<scalar_product_op<LhsScalar,RhsScalar> > { enum { ret = 1 }; };
+template<typename LhsScalar,typename RhsScalar> struct functor_allows_mixing_real_and_complex<scalar_conj_product_op<LhsScalar,RhsScalar> > { enum { ret = 1 }; };
+
+
+/** \internal
+  * \brief Template functor to add a scalar to a fixed other one
+  * \sa class CwiseUnaryOp, Array::operator+
+  */
+/* If you wonder why doing the pset1() in packetOp() is an optimization check scalar_multiple_op */
+template<typename Scalar>
+struct scalar_add_op {
+  typedef typename packet_traits<Scalar>::type Packet;
+  // FIXME default copy constructors seems bugged with std::complex<>
+  inline scalar_add_op(const scalar_add_op& other) : m_other(other.m_other) { }
+  inline scalar_add_op(const Scalar& other) : m_other(other) { }
+  inline Scalar operator() (const Scalar& a) const { return a + m_other; }
+  inline const Packet packetOp(const Packet& a) const
+  { return internal::padd(a, pset1<Packet>(m_other)); }
+  const Scalar m_other;
+};
+template<typename Scalar>
+struct functor_traits<scalar_add_op<Scalar> >
+{ enum { Cost = NumTraits<Scalar>::AddCost, PacketAccess = packet_traits<Scalar>::HasAdd }; };
+
+/** \internal
+  * \brief Template functor to compute the square root of a scalar
+  * \sa class CwiseUnaryOp, Cwise::sqrt()
+  */
+template<typename Scalar> struct scalar_sqrt_op {
+  EIGEN_EMPTY_STRUCT_CTOR(scalar_sqrt_op)
+  inline const Scalar operator() (const Scalar& a) const { return internal::sqrt(a); }
+  typedef typename packet_traits<Scalar>::type Packet;
+  inline Packet packetOp(const Packet& a) const { return internal::psqrt(a); }
+};
+template<typename Scalar>
+struct functor_traits<scalar_sqrt_op<Scalar> >
+{ enum {
+    Cost = 5 * NumTraits<Scalar>::MulCost,
+    PacketAccess = packet_traits<Scalar>::HasSqrt
+  };
+};
+
+/** \internal
+  * \brief Template functor to compute the cosine of a scalar
+  * \sa class CwiseUnaryOp, ArrayBase::cos()
+  */
+template<typename Scalar> struct scalar_cos_op {
+  EIGEN_EMPTY_STRUCT_CTOR(scalar_cos_op)
+  inline Scalar operator() (const Scalar& a) const { return internal::cos(a); }
+  typedef typename packet_traits<Scalar>::type Packet;
+  inline Packet packetOp(const Packet& a) const { return internal::pcos(a); }
+};
+template<typename Scalar>
+struct functor_traits<scalar_cos_op<Scalar> >
+{
+  enum {
+    Cost = 5 * NumTraits<Scalar>::MulCost,
+    PacketAccess = packet_traits<Scalar>::HasCos
+  };
+};
+
+/** \internal
+  * \brief Template functor to compute the sine of a scalar
+  * \sa class CwiseUnaryOp, ArrayBase::sin()
+  */
+template<typename Scalar> struct scalar_sin_op {
+  EIGEN_EMPTY_STRUCT_CTOR(scalar_sin_op)
+  inline const Scalar operator() (const Scalar& a) const { return internal::sin(a); }
+  typedef typename packet_traits<Scalar>::type Packet;
+  inline Packet packetOp(const Packet& a) const { return internal::psin(a); }
+};
+template<typename Scalar>
+struct functor_traits<scalar_sin_op<Scalar> >
+{
+  enum {
+    Cost = 5 * NumTraits<Scalar>::MulCost,
+    PacketAccess = packet_traits<Scalar>::HasSin
+  };
+};
+
+
+/** \internal
+  * \brief Template functor to compute the tan of a scalar
+  * \sa class CwiseUnaryOp, ArrayBase::tan()
+  */
+template<typename Scalar> struct scalar_tan_op {
+  EIGEN_EMPTY_STRUCT_CTOR(scalar_tan_op)
+  inline const Scalar operator() (const Scalar& a) const { return internal::tan(a); }
+  typedef typename packet_traits<Scalar>::type Packet;
+  inline Packet packetOp(const Packet& a) const { return internal::ptan(a); }
+};
+template<typename Scalar>
+struct functor_traits<scalar_tan_op<Scalar> >
+{
+  enum {
+    Cost = 5 * NumTraits<Scalar>::MulCost,
+    PacketAccess = packet_traits<Scalar>::HasTan
+  };
+};
+
+/** \internal
+  * \brief Template functor to compute the arc cosine of a scalar
+  * \sa class CwiseUnaryOp, ArrayBase::acos()
+  */
+template<typename Scalar> struct scalar_acos_op {
+  EIGEN_EMPTY_STRUCT_CTOR(scalar_acos_op)
+  inline const Scalar operator() (const Scalar& a) const { return internal::acos(a); }
+  typedef typename packet_traits<Scalar>::type Packet;
+  inline Packet packetOp(const Packet& a) const { return internal::pacos(a); }
+};
+template<typename Scalar>
+struct functor_traits<scalar_acos_op<Scalar> >
+{
+  enum {
+    Cost = 5 * NumTraits<Scalar>::MulCost,
+    PacketAccess = packet_traits<Scalar>::HasACos
+  };
+};
+
+/** \internal
+  * \brief Template functor to compute the arc sine of a scalar
+  * \sa class CwiseUnaryOp, ArrayBase::asin()
+  */
+template<typename Scalar> struct scalar_asin_op {
+  EIGEN_EMPTY_STRUCT_CTOR(scalar_asin_op)
+  inline const Scalar operator() (const Scalar& a) const { return internal::asin(a); }
+  typedef typename packet_traits<Scalar>::type Packet;
+  inline Packet packetOp(const Packet& a) const { return internal::pasin(a); }
+};
+template<typename Scalar>
+struct functor_traits<scalar_asin_op<Scalar> >
+{
+  enum {
+    Cost = 5 * NumTraits<Scalar>::MulCost,
+    PacketAccess = packet_traits<Scalar>::HasASin
+  };
+};
+
+/** \internal
+  * \brief Template functor to raise a scalar to a power
+  * \sa class CwiseUnaryOp, Cwise::pow
+  */
+template<typename Scalar>
+struct scalar_pow_op {
+  // FIXME default copy constructors seems bugged with std::complex<>
+  inline scalar_pow_op(const scalar_pow_op& other) : m_exponent(other.m_exponent) { }
+  inline scalar_pow_op(const Scalar& exponent) : m_exponent(exponent) {}
+  inline Scalar operator() (const Scalar& a) const { return internal::pow(a, m_exponent); }
+  const Scalar m_exponent;
+};
+template<typename Scalar>
+struct functor_traits<scalar_pow_op<Scalar> >
+{ enum { Cost = 5 * NumTraits<Scalar>::MulCost, PacketAccess = false }; };
+
+/** \internal
+  * \brief Template functor to compute the quotient between a scalar and array entries.
+  * \sa class CwiseUnaryOp, Cwise::inverse()
+  */
+template<typename Scalar>
+struct scalar_inverse_mult_op {
+  scalar_inverse_mult_op(const Scalar& other) : m_other(other) {}
+  inline Scalar operator() (const Scalar& a) const { return m_other / a; }
+  template<typename Packet>
+  inline const Packet packetOp(const Packet& a) const
+  { return internal::pdiv(pset1<Packet>(m_other),a); }
+  Scalar m_other;
+};
+
+/** \internal
+  * \brief Template functor to compute the inverse of a scalar
+  * \sa class CwiseUnaryOp, Cwise::inverse()
+  */
+template<typename Scalar>
+struct scalar_inverse_op {
+  EIGEN_EMPTY_STRUCT_CTOR(scalar_inverse_op)
+  inline Scalar operator() (const Scalar& a) const { return Scalar(1)/a; }
+  template<typename Packet>
+  inline const Packet packetOp(const Packet& a) const
+  { return internal::pdiv(pset1<Packet>(Scalar(1)),a); }
+};
+template<typename Scalar>
+struct functor_traits<scalar_inverse_op<Scalar> >
+{ enum { Cost = NumTraits<Scalar>::MulCost, PacketAccess = packet_traits<Scalar>::HasDiv }; };
+
+/** \internal
+  * \brief Template functor to compute the square of a scalar
+  * \sa class CwiseUnaryOp, Cwise::square()
+  */
+template<typename Scalar>
+struct scalar_square_op {
+  EIGEN_EMPTY_STRUCT_CTOR(scalar_square_op)
+  inline Scalar operator() (const Scalar& a) const { return a*a; }
+  template<typename Packet>
+  inline const Packet packetOp(const Packet& a) const
+  { return internal::pmul(a,a); }
+};
+template<typename Scalar>
+struct functor_traits<scalar_square_op<Scalar> >
+{ enum { Cost = NumTraits<Scalar>::MulCost, PacketAccess = packet_traits<Scalar>::HasMul }; };
+
+/** \internal
+  * \brief Template functor to compute the cube of a scalar
+  * \sa class CwiseUnaryOp, Cwise::cube()
+  */
+template<typename Scalar>
+struct scalar_cube_op {
+  EIGEN_EMPTY_STRUCT_CTOR(scalar_cube_op)
+  inline Scalar operator() (const Scalar& a) const { return a*a*a; }
+  template<typename Packet>
+  inline const Packet packetOp(const Packet& a) const
+  { return internal::pmul(a,pmul(a,a)); }
+};
+template<typename Scalar>
+struct functor_traits<scalar_cube_op<Scalar> >
+{ enum { Cost = 2*NumTraits<Scalar>::MulCost, PacketAccess = packet_traits<Scalar>::HasMul }; };
+
+// default functor traits for STL functors:
+
+template<typename T>
+struct functor_traits<std::multiplies<T> >
+{ enum { Cost = NumTraits<T>::MulCost, PacketAccess = false }; };
+
+template<typename T>
+struct functor_traits<std::divides<T> >
+{ enum { Cost = NumTraits<T>::MulCost, PacketAccess = false }; };
+
+template<typename T>
+struct functor_traits<std::plus<T> >
+{ enum { Cost = NumTraits<T>::AddCost, PacketAccess = false }; };
+
+template<typename T>
+struct functor_traits<std::minus<T> >
+{ enum { Cost = NumTraits<T>::AddCost, PacketAccess = false }; };
+
+template<typename T>
+struct functor_traits<std::negate<T> >
+{ enum { Cost = NumTraits<T>::AddCost, PacketAccess = false }; };
+
+template<typename T>
+struct functor_traits<std::logical_or<T> >
+{ enum { Cost = 1, PacketAccess = false }; };
+
+template<typename T>
+struct functor_traits<std::logical_and<T> >
+{ enum { Cost = 1, PacketAccess = false }; };
+
+template<typename T>
+struct functor_traits<std::logical_not<T> >
+{ enum { Cost = 1, PacketAccess = false }; };
+
+template<typename T>
+struct functor_traits<std::greater<T> >
+{ enum { Cost = 1, PacketAccess = false }; };
+
+template<typename T>
+struct functor_traits<std::less<T> >
+{ enum { Cost = 1, PacketAccess = false }; };
+
+template<typename T>
+struct functor_traits<std::greater_equal<T> >
+{ enum { Cost = 1, PacketAccess = false }; };
+
+template<typename T>
+struct functor_traits<std::less_equal<T> >
+{ enum { Cost = 1, PacketAccess = false }; };
+
+template<typename T>
+struct functor_traits<std::equal_to<T> >
+{ enum { Cost = 1, PacketAccess = false }; };
+
+template<typename T>
+struct functor_traits<std::not_equal_to<T> >
+{ enum { Cost = 1, PacketAccess = false }; };
+
+#if __cplusplus < 201703L
+template<typename T>
+struct functor_traits<std::binder2nd<T> >
+{ enum { Cost = functor_traits<T>::Cost, PacketAccess = false }; };
+
+template<typename T>
+struct functor_traits<std::binder1st<T> >
+{ enum { Cost = functor_traits<T>::Cost, PacketAccess = false }; };
+
+template<typename T>
+struct functor_traits<std::unary_negate<T> >
+{ enum { Cost = 1 + functor_traits<T>::Cost, PacketAccess = false }; };
+
+template<typename T>
+struct functor_traits<std::binary_negate<T> >
+{ enum { Cost = 1 + functor_traits<T>::Cost, PacketAccess = false }; };
+#endif
+
+#ifdef EIGEN_STDEXT_SUPPORT
+
+template<typename T0,typename T1>
+struct functor_traits<std::project1st<T0,T1> >
+{ enum { Cost = 0, PacketAccess = false }; };
+
+template<typename T0,typename T1>
+struct functor_traits<std::project2nd<T0,T1> >
+{ enum { Cost = 0, PacketAccess = false }; };
+
+template<typename T0,typename T1>
+struct functor_traits<std::select2nd<std::pair<T0,T1> > >
+{ enum { Cost = 0, PacketAccess = false }; };
+
+template<typename T0,typename T1>
+struct functor_traits<std::select1st<std::pair<T0,T1> > >
+{ enum { Cost = 0, PacketAccess = false }; };
+
+template<typename T0,typename T1>
+struct functor_traits<std::unary_compose<T0,T1> >
+{ enum { Cost = functor_traits<T0>::Cost + functor_traits<T1>::Cost, PacketAccess = false }; };
+
+template<typename T0,typename T1,typename T2>
+struct functor_traits<std::binary_compose<T0,T1,T2> >
+{ enum { Cost = functor_traits<T0>::Cost + functor_traits<T1>::Cost + functor_traits<T2>::Cost, PacketAccess = false }; };
+
+#endif // EIGEN_STDEXT_SUPPORT
+
+// allow to add new functors and specializations of functor_traits from outside Eigen.
+// this macro is really needed because functor_traits must be specialized after it is declared but before it is used...
+#ifdef EIGEN_FUNCTORS_PLUGIN
+#include EIGEN_FUNCTORS_PLUGIN
+#endif
+
+} // end namespace internal
+
+} // end namespace Eigen
+
+#endif // EIGEN_FUNCTORS_H
diff --git a/Util/Patches/Eigen3.1.0/MatrixCwiseUnaryOps.h b/Util/Patches/Eigen3.1.0/MatrixCwiseUnaryOps.h
new file mode 100644
index 000000000..297bfb5d3
--- /dev/null
+++ b/Util/Patches/Eigen3.1.0/MatrixCwiseUnaryOps.h
@@ -0,0 +1,85 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra.
+//
+// Copyright (C) 2008-2009 Gael Guennebaud <gael.guennebaud@inria.fr>
+// Copyright (C) 2006-2008 Benoit Jacob <jacob.benoit.1@gmail.com>
+//
+// Eigen is free software; you can redistribute it and/or
+// modify it under the terms of the GNU Lesser General Public
+// License as published by the Free Software Foundation; either
+// version 3 of the License, or (at your option) any later version.
+//
+// Alternatively, you can redistribute it and/or
+// modify it under the terms of the GNU General Public License as
+// published by the Free Software Foundation; either version 2 of
+// the License, or (at your option) any later version.
+//
+// Eigen is distributed in the hope that it will be useful, but WITHOUT ANY
+// WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
+// FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License or the
+// GNU General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public
+// License and a copy of the GNU General Public License along with
+// Eigen. If not, see <http://www.gnu.org/licenses/>.
+
+// This file is a base class plugin containing matrix specifics coefficient wise functions.
+
+
+/** \returns an expression of the coefficient-wise absolute value of \c *this
+  *
+  * Example: \include MatrixBase_cwiseAbs.cpp
+  * Output: \verbinclude MatrixBase_cwiseAbs.out
+  *
+  * \sa cwiseAbs2()
+  */
+EIGEN_STRONG_INLINE const CwiseUnaryOp<internal::scalar_abs_op<Scalar>, const Derived>
+cwiseAbs() const { return derived(); }
+
+/** \returns an expression of the coefficient-wise squared absolute value of \c *this
+  *
+  * Example: \include MatrixBase_cwiseAbs2.cpp
+  * Output: \verbinclude MatrixBase_cwiseAbs2.out
+  *
+  * \sa cwiseAbs()
+  */
+EIGEN_STRONG_INLINE const CwiseUnaryOp<internal::scalar_abs2_op<Scalar>, const Derived>
+cwiseAbs2() const { return derived(); }
+
+/** \returns an expression of the coefficient-wise square root of *this.
+  *
+  * Example: \include MatrixBase_cwiseSqrt.cpp
+  * Output: \verbinclude MatrixBase_cwiseSqrt.out
+  *
+  * \sa cwisePow(), cwiseSquare()
+  */
+inline const CwiseUnaryOp<internal::scalar_sqrt_op<Scalar>, const Derived>
+cwiseSqrt() const { return derived(); }
+
+/** \returns an expression of the coefficient-wise inverse of *this.
+  *
+  * Example: \include MatrixBase_cwiseInverse.cpp
+  * Output: \verbinclude MatrixBase_cwiseInverse.out
+  *
+  * \sa cwiseProduct()
+  */
+inline const CwiseUnaryOp<internal::scalar_inverse_op<Scalar>, const Derived>
+cwiseInverse() const { return derived(); }
+
+#if __cplusplus < 201703L
+/** \returns an expression of the coefficient-wise == operator of \c *this and a scalar \a s
+  *
+  * \warning this performs an exact comparison, which is generally a bad idea with floating-point types.
+  * In order to check for equality between two vectors or matrices with floating-point coefficients, it is
+  * generally a far better idea to use a fuzzy comparison as provided by isApprox() and
+  * isMuchSmallerThan().
+  *
+  * \sa cwiseEqual(const MatrixBase<OtherDerived> &) const
+  */
+inline const CwiseUnaryOp<std::binder1st<std::equal_to<Scalar> >, const Derived>
+cwiseEqual(const Scalar& s) const
+{
+  return CwiseUnaryOp<std::binder1st<std::equal_to<Scalar> >,const Derived>
+          (derived(), std::bind1st(std::equal_to<Scalar>(), s));
+}
+#endif
\ No newline at end of file
diff --git a/Util/Patches/Eigen3.1.0/PacketMath.h b/Util/Patches/Eigen3.1.0/PacketMath.h
new file mode 100644
index 000000000..c618a2cc3
--- /dev/null
+++ b/Util/Patches/Eigen3.1.0/PacketMath.h
@@ -0,0 +1,647 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra.
+//
+// Copyright (C) 2008-2009 Gael Guennebaud <gael.guennebaud@inria.fr>
+//
+// Eigen is free software; you can redistribute it and/or
+// modify it under the terms of the GNU Lesser General Public
+// License as published by the Free Software Foundation; either
+// version 3 of the License, or (at your option) any later version.
+//
+// Alternatively, you can redistribute it and/or
+// modify it under the terms of the GNU General Public License as
+// published by the Free Software Foundation; either version 2 of
+// the License, or (at your option) any later version.
+//
+// Eigen is distributed in the hope that it will be useful, but WITHOUT ANY
+// WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
+// FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License or the
+// GNU General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public
+// License and a copy of the GNU General Public License along with
+// Eigen. If not, see <http://www.gnu.org/licenses/>.
+
+#ifndef EIGEN_PACKET_MATH_SSE_H
+#define EIGEN_PACKET_MATH_SSE_H
+
+namespace Eigen {
+
+namespace internal {
+
+#ifndef EIGEN_CACHEFRIENDLY_PRODUCT_THRESHOLD
+#define EIGEN_CACHEFRIENDLY_PRODUCT_THRESHOLD 8
+#endif
+
+#ifndef EIGEN_ARCH_DEFAULT_NUMBER_OF_REGISTERS
+#define EIGEN_ARCH_DEFAULT_NUMBER_OF_REGISTERS (2*sizeof(void*))
+#endif
+
+typedef __m128  Packet4f;
+typedef __m128i Packet4i;
+typedef __m128d Packet2d;
+
+template<> struct is_arithmetic<__m128>  { enum { value = true }; };
+template<> struct is_arithmetic<__m128i> { enum { value = true }; };
+template<> struct is_arithmetic<__m128d> { enum { value = true }; };
+
+#define vec4f_swizzle1(v,p,q,r,s) \
+  (_mm_castsi128_ps(_mm_shuffle_epi32( _mm_castps_si128(v), ((s)<<6|(r)<<4|(q)<<2|(p)))))
+
+#define vec4i_swizzle1(v,p,q,r,s) \
+  (_mm_shuffle_epi32( v, ((s)<<6|(r)<<4|(q)<<2|(p))))
+
+#define vec2d_swizzle1(v,p,q) \
+  (_mm_castsi128_pd(_mm_shuffle_epi32( _mm_castpd_si128(v), ((q*2+1)<<6|(q*2)<<4|(p*2+1)<<2|(p*2)))))
+  
+#define vec4f_swizzle2(a,b,p,q,r,s) \
+  (_mm_shuffle_ps( (a), (b), ((s)<<6|(r)<<4|(q)<<2|(p))))
+
+#define vec4i_swizzle2(a,b,p,q,r,s) \
+  (_mm_castps_si128( (_mm_shuffle_ps( _mm_castsi128_ps(a), _mm_castsi128_ps(b), ((s)<<6|(r)<<4|(q)<<2|(p))))))
+
+#define _EIGEN_DECLARE_CONST_Packet4f(NAME,X) \
+  const Packet4f p4f_##NAME = pset1<Packet4f>(X)
+
+#define _EIGEN_DECLARE_CONST_Packet4f_FROM_INT(NAME,X) \
+  const Packet4f p4f_##NAME = _mm_castsi128_ps(pset1<Packet4i>(X))
+
+#define _EIGEN_DECLARE_CONST_Packet4i(NAME,X) \
+  const Packet4i p4i_##NAME = pset1<Packet4i>(X)
+
+
+template<> struct packet_traits<float>  : default_packet_traits
+{
+  typedef Packet4f type;
+  enum {
+    Vectorizable = 1,
+    AlignedOnScalar = 1,
+    size=4,
+
+    HasDiv    = 1,
+    HasSin  = EIGEN_FAST_MATH,
+    HasCos  = EIGEN_FAST_MATH,
+    HasLog  = 1,
+    HasExp  = 1,
+    HasSqrt = 1
+  };
+};
+template<> struct packet_traits<double> : default_packet_traits
+{
+  typedef Packet2d type;
+  enum {
+    Vectorizable = 1,
+    AlignedOnScalar = 1,
+    size=2,
+
+    HasDiv    = 1
+  };
+};
+template<> struct packet_traits<int>    : default_packet_traits
+{
+  typedef Packet4i type;
+  enum {
+    // FIXME check the Has*
+    Vectorizable = 1,
+    AlignedOnScalar = 1,
+    size=4
+  };
+};
+
+template<> struct unpacket_traits<Packet4f> { typedef float  type; enum {size=4}; };
+template<> struct unpacket_traits<Packet2d> { typedef double type; enum {size=2}; };
+template<> struct unpacket_traits<Packet4i> { typedef int    type; enum {size=4}; };
+
+#if defined(_MSC_VER) && (_MSC_VER==1500)
+// Workaround MSVC 9 internal compiler error.
+// TODO: It has been detected with win64 builds (amd64), so let's check whether it also happens in 32bits+SSE mode
+// TODO: let's check whether there does not exist a better fix, like adding a pset0() function. (it crashed on pset1(0)).
+template<> EIGEN_STRONG_INLINE Packet4f pset1<Packet4f>(const float&  from) { return _mm_set_ps(from,from,from,from); }
+template<> EIGEN_STRONG_INLINE Packet2d pset1<Packet2d>(const double& from) { return _mm_set_pd(from,from); }
+template<> EIGEN_STRONG_INLINE Packet4i pset1<Packet4i>(const int&    from) { return _mm_set_epi32(from,from,from,from); }
+#else
+template<> EIGEN_STRONG_INLINE Packet4f pset1<Packet4f>(const float&  from) { return _mm_set1_ps(from); }
+template<> EIGEN_STRONG_INLINE Packet2d pset1<Packet2d>(const double& from) { return _mm_set1_pd(from); }
+template<> EIGEN_STRONG_INLINE Packet4i pset1<Packet4i>(const int&    from) { return _mm_set1_epi32(from); }
+#endif
+
+template<> EIGEN_STRONG_INLINE Packet4f plset<float>(const float& a) { return _mm_add_ps(pset1<Packet4f>(a), _mm_set_ps(3,2,1,0)); }
+template<> EIGEN_STRONG_INLINE Packet2d plset<double>(const double& a) { return _mm_add_pd(pset1<Packet2d>(a),_mm_set_pd(1,0)); }
+template<> EIGEN_STRONG_INLINE Packet4i plset<int>(const int& a) { return _mm_add_epi32(pset1<Packet4i>(a),_mm_set_epi32(3,2,1,0)); }
+
+template<> EIGEN_STRONG_INLINE Packet4f padd<Packet4f>(const Packet4f& a, const Packet4f& b) { return _mm_add_ps(a,b); }
+template<> EIGEN_STRONG_INLINE Packet2d padd<Packet2d>(const Packet2d& a, const Packet2d& b) { return _mm_add_pd(a,b); }
+template<> EIGEN_STRONG_INLINE Packet4i padd<Packet4i>(const Packet4i& a, const Packet4i& b) { return _mm_add_epi32(a,b); }
+
+template<> EIGEN_STRONG_INLINE Packet4f psub<Packet4f>(const Packet4f& a, const Packet4f& b) { return _mm_sub_ps(a,b); }
+template<> EIGEN_STRONG_INLINE Packet2d psub<Packet2d>(const Packet2d& a, const Packet2d& b) { return _mm_sub_pd(a,b); }
+template<> EIGEN_STRONG_INLINE Packet4i psub<Packet4i>(const Packet4i& a, const Packet4i& b) { return _mm_sub_epi32(a,b); }
+
+template<> EIGEN_STRONG_INLINE Packet4f pnegate(const Packet4f& a)
+{
+  const Packet4f mask = _mm_castsi128_ps(_mm_setr_epi32(0x80000000,0x80000000,0x80000000,0x80000000));
+  return _mm_xor_ps(a,mask);
+}
+template<> EIGEN_STRONG_INLINE Packet2d pnegate(const Packet2d& a)
+{
+  const Packet2d mask = _mm_castsi128_pd(_mm_setr_epi32(0x0,0x80000000,0x0,0x80000000));
+  return _mm_xor_pd(a,mask);
+}
+template<> EIGEN_STRONG_INLINE Packet4i pnegate(const Packet4i& a)
+{
+  return psub(_mm_setr_epi32(0,0,0,0), a);
+}
+
+template<> EIGEN_STRONG_INLINE Packet4f pmul<Packet4f>(const Packet4f& a, const Packet4f& b) { return _mm_mul_ps(a,b); }
+template<> EIGEN_STRONG_INLINE Packet2d pmul<Packet2d>(const Packet2d& a, const Packet2d& b) { return _mm_mul_pd(a,b); }
+template<> EIGEN_STRONG_INLINE Packet4i pmul<Packet4i>(const Packet4i& a, const Packet4i& b)
+{
+#ifdef EIGEN_VECTORIZE_SSE4_1
+  return _mm_mullo_epi32(a,b);
+#else
+  // this version is slightly faster than 4 scalar products
+  return vec4i_swizzle1(
+            vec4i_swizzle2(
+              _mm_mul_epu32(a,b),
+              _mm_mul_epu32(vec4i_swizzle1(a,1,0,3,2),
+                            vec4i_swizzle1(b,1,0,3,2)),
+              0,2,0,2),
+            0,2,1,3);
+#endif
+}
+
+template<> EIGEN_STRONG_INLINE Packet4f pdiv<Packet4f>(const Packet4f& a, const Packet4f& b) { return _mm_div_ps(a,b); }
+template<> EIGEN_STRONG_INLINE Packet2d pdiv<Packet2d>(const Packet2d& a, const Packet2d& b) { return _mm_div_pd(a,b); }
+template<> EIGEN_STRONG_INLINE Packet4i pdiv<Packet4i>(const Packet4i& /*a*/, const Packet4i& /*b*/)
+{ eigen_assert(false && "packet integer division are not supported by SSE");
+  return pset1<Packet4i>(0);
+}
+
+// for some weird raisons, it has to be overloaded for packet of integers
+template<> EIGEN_STRONG_INLINE Packet4i pmadd(const Packet4i& a, const Packet4i& b, const Packet4i& c) { return padd(pmul(a,b), c); }
+
+template<> EIGEN_STRONG_INLINE Packet4f pmin<Packet4f>(const Packet4f& a, const Packet4f& b) { return _mm_min_ps(a,b); }
+template<> EIGEN_STRONG_INLINE Packet2d pmin<Packet2d>(const Packet2d& a, const Packet2d& b) { return _mm_min_pd(a,b); }
+template<> EIGEN_STRONG_INLINE Packet4i pmin<Packet4i>(const Packet4i& a, const Packet4i& b)
+{
+  // after some bench, this version *is* faster than a scalar implementation
+  Packet4i mask = _mm_cmplt_epi32(a,b);
+  return _mm_or_si128(_mm_and_si128(mask,a),_mm_andnot_si128(mask,b));
+}
+
+template<> EIGEN_STRONG_INLINE Packet4f pmax<Packet4f>(const Packet4f& a, const Packet4f& b) { return _mm_max_ps(a,b); }
+template<> EIGEN_STRONG_INLINE Packet2d pmax<Packet2d>(const Packet2d& a, const Packet2d& b) { return _mm_max_pd(a,b); }
+template<> EIGEN_STRONG_INLINE Packet4i pmax<Packet4i>(const Packet4i& a, const Packet4i& b)
+{
+  // after some bench, this version *is* faster than a scalar implementation
+  Packet4i mask = _mm_cmpgt_epi32(a,b);
+  return _mm_or_si128(_mm_and_si128(mask,a),_mm_andnot_si128(mask,b));
+}
+
+template<> EIGEN_STRONG_INLINE Packet4f pand<Packet4f>(const Packet4f& a, const Packet4f& b) { return _mm_and_ps(a,b); }
+template<> EIGEN_STRONG_INLINE Packet2d pand<Packet2d>(const Packet2d& a, const Packet2d& b) { return _mm_and_pd(a,b); }
+template<> EIGEN_STRONG_INLINE Packet4i pand<Packet4i>(const Packet4i& a, const Packet4i& b) { return _mm_and_si128(a,b); }
+
+template<> EIGEN_STRONG_INLINE Packet4f por<Packet4f>(const Packet4f& a, const Packet4f& b) { return _mm_or_ps(a,b); }
+template<> EIGEN_STRONG_INLINE Packet2d por<Packet2d>(const Packet2d& a, const Packet2d& b) { return _mm_or_pd(a,b); }
+template<> EIGEN_STRONG_INLINE Packet4i por<Packet4i>(const Packet4i& a, const Packet4i& b) { return _mm_or_si128(a,b); }
+
+template<> EIGEN_STRONG_INLINE Packet4f pxor<Packet4f>(const Packet4f& a, const Packet4f& b) { return _mm_xor_ps(a,b); }
+template<> EIGEN_STRONG_INLINE Packet2d pxor<Packet2d>(const Packet2d& a, const Packet2d& b) { return _mm_xor_pd(a,b); }
+template<> EIGEN_STRONG_INLINE Packet4i pxor<Packet4i>(const Packet4i& a, const Packet4i& b) { return _mm_xor_si128(a,b); }
+
+template<> EIGEN_STRONG_INLINE Packet4f pandnot<Packet4f>(const Packet4f& a, const Packet4f& b) { return _mm_andnot_ps(a,b); }
+template<> EIGEN_STRONG_INLINE Packet2d pandnot<Packet2d>(const Packet2d& a, const Packet2d& b) { return _mm_andnot_pd(a,b); }
+template<> EIGEN_STRONG_INLINE Packet4i pandnot<Packet4i>(const Packet4i& a, const Packet4i& b) { return _mm_andnot_si128(a,b); }
+
+template<> EIGEN_STRONG_INLINE Packet4f pload<Packet4f>(const float*   from) { EIGEN_DEBUG_ALIGNED_LOAD return _mm_load_ps(from); }
+template<> EIGEN_STRONG_INLINE Packet2d pload<Packet2d>(const double*  from) { EIGEN_DEBUG_ALIGNED_LOAD return _mm_load_pd(from); }
+template<> EIGEN_STRONG_INLINE Packet4i pload<Packet4i>(const int*     from) { EIGEN_DEBUG_ALIGNED_LOAD return _mm_load_si128(reinterpret_cast<const Packet4i*>(from)); }
+
+#if defined(_MSC_VER)
+  template<> EIGEN_STRONG_INLINE Packet4f ploadu<Packet4f>(const float*  from) {
+    EIGEN_DEBUG_UNALIGNED_LOAD
+    #if (_MSC_VER==1600)
+    // NOTE Some version of MSVC10 generates bad code when using _mm_loadu_ps
+    // (i.e., it does not generate an unaligned load!!
+    // TODO On most architectures this version should also be faster than a single _mm_loadu_ps
+    // so we could also enable it for MSVC08 but first we have to make this later does not generate crap when doing so...
+    __m128 res = _mm_loadl_pi(_mm_set1_ps(0.0f), (const __m64*)(from));
+    res = _mm_loadh_pi(res, (const __m64*)(from+2));
+    return res;
+    #else
+    return _mm_loadu_ps(from);
+    #endif
+  }
+  template<> EIGEN_STRONG_INLINE Packet2d ploadu<Packet2d>(const double* from) { EIGEN_DEBUG_UNALIGNED_LOAD return _mm_loadu_pd(from); }
+  template<> EIGEN_STRONG_INLINE Packet4i ploadu<Packet4i>(const int*    from) { EIGEN_DEBUG_UNALIGNED_LOAD return _mm_loadu_si128(reinterpret_cast<const Packet4i*>(from)); }
+#else
+// Fast unaligned loads. Note that here we cannot directly use intrinsics: this would
+// require pointer casting to incompatible pointer types and leads to invalid code
+// because of the strict aliasing rule. The "dummy" stuff are required to enforce
+// a correct instruction dependency.
+// TODO: do the same for MSVC (ICC is compatible)
+// NOTE: with the code below, MSVC's compiler crashes!
+
+#if defined(__GNUC__) && defined(__i386__)
+  // bug 195: gcc/i386 emits weird x87 fldl/fstpl instructions for _mm_load_sd
+  #define EIGEN_AVOID_CUSTOM_UNALIGNED_LOADS 1
+#elif defined(__clang__)
+  // bug 201: Segfaults in __mm_loadh_pd with clang 2.8
+  #define EIGEN_AVOID_CUSTOM_UNALIGNED_LOADS 1
+#else
+  #define EIGEN_AVOID_CUSTOM_UNALIGNED_LOADS 0
+#endif
+
+template<> EIGEN_STRONG_INLINE Packet4f ploadu<Packet4f>(const float* from)
+{
+  EIGEN_DEBUG_UNALIGNED_LOAD
+#if EIGEN_AVOID_CUSTOM_UNALIGNED_LOADS
+  return _mm_loadu_ps(from);
+#else
+  __m128d res;
+  res =  _mm_load_sd((const double*)(from)) ;
+  res =  _mm_loadh_pd(res, (const double*)(from+2)) ;
+  return _mm_castpd_ps(res);
+#endif
+}
+template<> EIGEN_STRONG_INLINE Packet2d ploadu<Packet2d>(const double* from)
+{
+  EIGEN_DEBUG_UNALIGNED_LOAD
+#if EIGEN_AVOID_CUSTOM_UNALIGNED_LOADS
+  return _mm_loadu_pd(from);
+#else
+  __m128d res;
+  res = _mm_load_sd(from) ;
+  res = _mm_loadh_pd(res,from+1);
+  return res;
+#endif
+}
+template<> EIGEN_STRONG_INLINE Packet4i ploadu<Packet4i>(const int* from)
+{
+  EIGEN_DEBUG_UNALIGNED_LOAD
+#if EIGEN_AVOID_CUSTOM_UNALIGNED_LOADS
+  return _mm_loadu_si128(reinterpret_cast<const Packet4i*>(from));
+#else
+  __m128d res;
+  res =  _mm_load_sd((const double*)(from)) ;
+  res =  _mm_loadh_pd(res, (const double*)(from+2)) ;
+  return _mm_castpd_si128(res);
+#endif
+}
+#endif
+
+template<> EIGEN_STRONG_INLINE Packet4f ploaddup<Packet4f>(const float*   from)
+{
+  return vec4f_swizzle1(_mm_castpd_ps(_mm_load_sd(reinterpret_cast<const double*>(from))), 0, 0, 1, 1);
+}
+template<> EIGEN_STRONG_INLINE Packet2d ploaddup<Packet2d>(const double*  from)
+{ return pset1<Packet2d>(from[0]); }
+template<> EIGEN_STRONG_INLINE Packet4i ploaddup<Packet4i>(const int*     from)
+{
+  Packet4i tmp;
+  tmp = _mm_loadl_epi64(reinterpret_cast<const Packet4i*>(from));
+  return vec4i_swizzle1(tmp, 0, 0, 1, 1);
+}
+
+template<> EIGEN_STRONG_INLINE void pstore<float>(float*   to, const Packet4f& from) { EIGEN_DEBUG_ALIGNED_STORE _mm_store_ps(to, from); }
+template<> EIGEN_STRONG_INLINE void pstore<double>(double* to, const Packet2d& from) { EIGEN_DEBUG_ALIGNED_STORE _mm_store_pd(to, from); }
+template<> EIGEN_STRONG_INLINE void pstore<int>(int*       to, const Packet4i& from) { EIGEN_DEBUG_ALIGNED_STORE _mm_store_si128(reinterpret_cast<Packet4i*>(to), from); }
+
+template<> EIGEN_STRONG_INLINE void pstoreu<double>(double* to, const Packet2d& from) {
+  EIGEN_DEBUG_UNALIGNED_STORE
+  _mm_storel_pd((to), from);
+  _mm_storeh_pd((to+1), from);
+}
+template<> EIGEN_STRONG_INLINE void pstoreu<float>(float*  to, const Packet4f& from) { EIGEN_DEBUG_UNALIGNED_STORE pstoreu(reinterpret_cast<double*>(to), _mm_castps_pd(from)); }
+template<> EIGEN_STRONG_INLINE void pstoreu<int>(int*      to, const Packet4i& from) { EIGEN_DEBUG_UNALIGNED_STORE pstoreu(reinterpret_cast<double*>(to), _mm_castsi128_pd(from)); }
+
+// some compilers might be tempted to perform multiple moves instead of using a vector path.
+template<> EIGEN_STRONG_INLINE void pstore1<Packet4f>(float* to, const float& a)
+{
+  Packet4f pa = _mm_set_ss(a);
+  pstore(to, vec4f_swizzle1(pa,0,0,0,0));
+}
+// some compilers might be tempted to perform multiple moves instead of using a vector path.
+template<> EIGEN_STRONG_INLINE void pstore1<Packet2d>(double* to, const double& a)
+{
+  Packet2d pa = _mm_set_sd(a);
+  pstore(to, vec2d_swizzle1(pa,0,0));
+}
+
+template<> EIGEN_STRONG_INLINE void prefetch<float>(const float*   addr) { _mm_prefetch((const char*)(addr), _MM_HINT_T0); }
+template<> EIGEN_STRONG_INLINE void prefetch<double>(const double* addr) { _mm_prefetch((const char*)(addr), _MM_HINT_T0); }
+template<> EIGEN_STRONG_INLINE void prefetch<int>(const int*       addr) { _mm_prefetch((const char*)(addr), _MM_HINT_T0); }
+
+#if defined(_MSC_VER) && defined(_WIN64) && !defined(__INTEL_COMPILER)
+// The temporary variable fixes an internal compilation error in vs <= 2008 and a wrong-result bug in vs 2010
+// Direct of the struct members fixed bug #62.
+template<> EIGEN_STRONG_INLINE float  pfirst<Packet4f>(const Packet4f& a) { return a.m128_f32[0]; }
+template<> EIGEN_STRONG_INLINE double pfirst<Packet2d>(const Packet2d& a) { return a.m128d_f64[0]; }
+template<> EIGEN_STRONG_INLINE int    pfirst<Packet4i>(const Packet4i& a) { int x = _mm_cvtsi128_si32(a); return x; }
+#elif defined(_MSC_VER) && !defined(__INTEL_COMPILER)
+// The temporary variable fixes an internal compilation error in vs <= 2008 and a wrong-result bug in vs 2010
+template<> EIGEN_STRONG_INLINE float  pfirst<Packet4f>(const Packet4f& a) { float x = _mm_cvtss_f32(a); return x; }
+template<> EIGEN_STRONG_INLINE double pfirst<Packet2d>(const Packet2d& a) { double x = _mm_cvtsd_f64(a); return x; }
+template<> EIGEN_STRONG_INLINE int    pfirst<Packet4i>(const Packet4i& a) { int x = _mm_cvtsi128_si32(a); return x; }
+#else
+template<> EIGEN_STRONG_INLINE float  pfirst<Packet4f>(const Packet4f& a) { return _mm_cvtss_f32(a); }
+template<> EIGEN_STRONG_INLINE double pfirst<Packet2d>(const Packet2d& a) { return _mm_cvtsd_f64(a); }
+template<> EIGEN_STRONG_INLINE int    pfirst<Packet4i>(const Packet4i& a) { return _mm_cvtsi128_si32(a); }
+#endif
+
+template<> EIGEN_STRONG_INLINE Packet4f preverse(const Packet4f& a)
+{ return _mm_shuffle_ps(a,a,0x1B); }
+template<> EIGEN_STRONG_INLINE Packet2d preverse(const Packet2d& a)
+{ return _mm_shuffle_pd(a,a,0x1); }
+template<> EIGEN_STRONG_INLINE Packet4i preverse(const Packet4i& a)
+{ return _mm_shuffle_epi32(a,0x1B); }
+
+
+template<> EIGEN_STRONG_INLINE Packet4f pabs(const Packet4f& a)
+{
+  const Packet4f mask = _mm_castsi128_ps(_mm_setr_epi32(0x7FFFFFFF,0x7FFFFFFF,0x7FFFFFFF,0x7FFFFFFF));
+  return _mm_and_ps(a,mask);
+}
+template<> EIGEN_STRONG_INLINE Packet2d pabs(const Packet2d& a)
+{
+  const Packet2d mask = _mm_castsi128_pd(_mm_setr_epi32(0xFFFFFFFF,0x7FFFFFFF,0xFFFFFFFF,0x7FFFFFFF));
+  return _mm_and_pd(a,mask);
+}
+template<> EIGEN_STRONG_INLINE Packet4i pabs(const Packet4i& a)
+{
+  #ifdef EIGEN_VECTORIZE_SSSE3
+  return _mm_abs_epi32(a);
+  #else
+  Packet4i aux = _mm_srai_epi32(a,31);
+  return _mm_sub_epi32(_mm_xor_si128(a,aux),aux);
+  #endif
+}
+
+EIGEN_STRONG_INLINE void punpackp(Packet4f* vecs)
+{
+  vecs[1] = _mm_castsi128_ps(_mm_shuffle_epi32(_mm_castps_si128(vecs[0]), 0x55));
+  vecs[2] = _mm_castsi128_ps(_mm_shuffle_epi32(_mm_castps_si128(vecs[0]), 0xAA));
+  vecs[3] = _mm_castsi128_ps(_mm_shuffle_epi32(_mm_castps_si128(vecs[0]), 0xFF));
+  vecs[0] = _mm_castsi128_ps(_mm_shuffle_epi32(_mm_castps_si128(vecs[0]), 0x00));
+}
+
+#ifdef EIGEN_VECTORIZE_SSE3
+// TODO implement SSE2 versions as well as integer versions
+template<> EIGEN_STRONG_INLINE Packet4f preduxp<Packet4f>(const Packet4f* vecs)
+{
+  return _mm_hadd_ps(_mm_hadd_ps(vecs[0], vecs[1]),_mm_hadd_ps(vecs[2], vecs[3]));
+}
+template<> EIGEN_STRONG_INLINE Packet2d preduxp<Packet2d>(const Packet2d* vecs)
+{
+  return _mm_hadd_pd(vecs[0], vecs[1]);
+}
+// SSSE3 version:
+// EIGEN_STRONG_INLINE Packet4i preduxp(const Packet4i* vecs)
+// {
+//   return _mm_hadd_epi32(_mm_hadd_epi32(vecs[0], vecs[1]),_mm_hadd_epi32(vecs[2], vecs[3]));
+// }
+
+template<> EIGEN_STRONG_INLINE float predux<Packet4f>(const Packet4f& a)
+{
+  Packet4f tmp0 = _mm_hadd_ps(a,a);
+  return pfirst(_mm_hadd_ps(tmp0, tmp0));
+}
+
+template<> EIGEN_STRONG_INLINE double predux<Packet2d>(const Packet2d& a) { return pfirst(_mm_hadd_pd(a, a)); }
+
+// SSSE3 version:
+// EIGEN_STRONG_INLINE float predux(const Packet4i& a)
+// {
+//   Packet4i tmp0 = _mm_hadd_epi32(a,a);
+//   return pfirst(_mm_hadd_epi32(tmp0, tmp0));
+// }
+#else
+// SSE2 versions
+template<> EIGEN_STRONG_INLINE float predux<Packet4f>(const Packet4f& a)
+{
+  Packet4f tmp = _mm_add_ps(a, _mm_movehl_ps(a,a));
+  return pfirst(_mm_add_ss(tmp, _mm_shuffle_ps(tmp,tmp, 1)));
+}
+template<> EIGEN_STRONG_INLINE double predux<Packet2d>(const Packet2d& a)
+{
+  return pfirst(_mm_add_sd(a, _mm_unpackhi_pd(a,a)));
+}
+
+template<> EIGEN_STRONG_INLINE Packet4f preduxp<Packet4f>(const Packet4f* vecs)
+{
+  Packet4f tmp0, tmp1, tmp2;
+  tmp0 = _mm_unpacklo_ps(vecs[0], vecs[1]);
+  tmp1 = _mm_unpackhi_ps(vecs[0], vecs[1]);
+  tmp2 = _mm_unpackhi_ps(vecs[2], vecs[3]);
+  tmp0 = _mm_add_ps(tmp0, tmp1);
+  tmp1 = _mm_unpacklo_ps(vecs[2], vecs[3]);
+  tmp1 = _mm_add_ps(tmp1, tmp2);
+  tmp2 = _mm_movehl_ps(tmp1, tmp0);
+  tmp0 = _mm_movelh_ps(tmp0, tmp1);
+  return _mm_add_ps(tmp0, tmp2);
+}
+
+template<> EIGEN_STRONG_INLINE Packet2d preduxp<Packet2d>(const Packet2d* vecs)
+{
+  return _mm_add_pd(_mm_unpacklo_pd(vecs[0], vecs[1]), _mm_unpackhi_pd(vecs[0], vecs[1]));
+}
+#endif  // SSE3
+
+template<> EIGEN_STRONG_INLINE int predux<Packet4i>(const Packet4i& a)
+{
+  Packet4i tmp = _mm_add_epi32(a, _mm_unpackhi_epi64(a,a));
+  return pfirst(tmp) + pfirst(_mm_shuffle_epi32(tmp, 1));
+}
+
+template<> EIGEN_STRONG_INLINE Packet4i preduxp<Packet4i>(const Packet4i* vecs)
+{
+  Packet4i tmp0, tmp1, tmp2;
+  tmp0 = _mm_unpacklo_epi32(vecs[0], vecs[1]);
+  tmp1 = _mm_unpackhi_epi32(vecs[0], vecs[1]);
+  tmp2 = _mm_unpackhi_epi32(vecs[2], vecs[3]);
+  tmp0 = _mm_add_epi32(tmp0, tmp1);
+  tmp1 = _mm_unpacklo_epi32(vecs[2], vecs[3]);
+  tmp1 = _mm_add_epi32(tmp1, tmp2);
+  tmp2 = _mm_unpacklo_epi64(tmp0, tmp1);
+  tmp0 = _mm_unpackhi_epi64(tmp0, tmp1);
+  return _mm_add_epi32(tmp0, tmp2);
+}
+
+// Other reduction functions:
+
+// mul
+template<> EIGEN_STRONG_INLINE float predux_mul<Packet4f>(const Packet4f& a)
+{
+  Packet4f tmp = _mm_mul_ps(a, _mm_movehl_ps(a,a));
+  return pfirst(_mm_mul_ss(tmp, _mm_shuffle_ps(tmp,tmp, 1)));
+}
+template<> EIGEN_STRONG_INLINE double predux_mul<Packet2d>(const Packet2d& a)
+{
+  return pfirst(_mm_mul_sd(a, _mm_unpackhi_pd(a,a)));
+}
+template<> EIGEN_STRONG_INLINE int predux_mul<Packet4i>(const Packet4i& a)
+{
+  // after some experiments, it is seems this is the fastest way to implement it
+  // for GCC (eg., reusing pmul is very slow !)
+  // TODO try to call _mm_mul_epu32 directly
+  EIGEN_ALIGN16 int aux[4];
+  pstore(aux, a);
+  return  (aux[0] * aux[1]) * (aux[2] * aux[3]);;
+}
+
+// min
+template<> EIGEN_STRONG_INLINE float predux_min<Packet4f>(const Packet4f& a)
+{
+  Packet4f tmp = _mm_min_ps(a, _mm_movehl_ps(a,a));
+  return pfirst(_mm_min_ss(tmp, _mm_shuffle_ps(tmp,tmp, 1)));
+}
+template<> EIGEN_STRONG_INLINE double predux_min<Packet2d>(const Packet2d& a)
+{
+  return pfirst(_mm_min_sd(a, _mm_unpackhi_pd(a,a)));
+}
+template<> EIGEN_STRONG_INLINE int predux_min<Packet4i>(const Packet4i& a)
+{
+  // after some experiments, it is seems this is the fastest way to implement it
+  // for GCC (eg., it does not like using std::min after the pstore !!)
+  EIGEN_ALIGN16 int aux[4];
+  pstore(aux, a);
+  int aux0 = aux[0]<aux[1] ? aux[0] : aux[1];
+  int aux2 = aux[2]<aux[3] ? aux[2] : aux[3];
+  return aux0<aux2 ? aux0 : aux2;
+}
+
+// max
+template<> EIGEN_STRONG_INLINE float predux_max<Packet4f>(const Packet4f& a)
+{
+  Packet4f tmp = _mm_max_ps(a, _mm_movehl_ps(a,a));
+  return pfirst(_mm_max_ss(tmp, _mm_shuffle_ps(tmp,tmp, 1)));
+}
+template<> EIGEN_STRONG_INLINE double predux_max<Packet2d>(const Packet2d& a)
+{
+  return pfirst(_mm_max_sd(a, _mm_unpackhi_pd(a,a)));
+}
+template<> EIGEN_STRONG_INLINE int predux_max<Packet4i>(const Packet4i& a)
+{
+  // after some experiments, it is seems this is the fastest way to implement it
+  // for GCC (eg., it does not like using std::min after the pstore !!)
+  EIGEN_ALIGN16 int aux[4];
+  pstore(aux, a);
+  int aux0 = aux[0]>aux[1] ? aux[0] : aux[1];
+  int aux2 = aux[2]>aux[3] ? aux[2] : aux[3];
+  return aux0>aux2 ? aux0 : aux2;
+}
+
+#if (defined __GNUC__)
+// template <> EIGEN_STRONG_INLINE Packet4f pmadd(const Packet4f&  a, const Packet4f&  b, const Packet4f&  c)
+// {
+//   Packet4f res = b;
+//   asm("mulps %[a], %[b] \n\taddps %[c], %[b]" : [b] "+x" (res) : [a] "x" (a), [c] "x" (c));
+//   return res;
+// }
+// EIGEN_STRONG_INLINE Packet4i _mm_alignr_epi8(const Packet4i&  a, const Packet4i&  b, const int i)
+// {
+//   Packet4i res = a;
+//   asm("palignr %[i], %[a], %[b] " : [b] "+x" (res) : [a] "x" (a), [i] "i" (i));
+//   return res;
+// }
+#endif
+
+#ifdef EIGEN_VECTORIZE_SSSE3
+// SSSE3 versions
+template<int Offset>
+struct palign_impl<Offset,Packet4f>
+{
+  static EIGEN_STRONG_INLINE void run(Packet4f& first, const Packet4f& second)
+  {
+    if (Offset!=0)
+      first = _mm_castsi128_ps(_mm_alignr_epi8(_mm_castps_si128(second), _mm_castps_si128(first), Offset*4));
+  }
+};
+
+template<int Offset>
+struct palign_impl<Offset,Packet4i>
+{
+  static EIGEN_STRONG_INLINE void run(Packet4i& first, const Packet4i& second)
+  {
+    if (Offset!=0)
+      first = _mm_alignr_epi8(second,first, Offset*4);
+  }
+};
+
+template<int Offset>
+struct palign_impl<Offset,Packet2d>
+{
+  static EIGEN_STRONG_INLINE void run(Packet2d& first, const Packet2d& second)
+  {
+    if (Offset==1)
+      first = _mm_castsi128_pd(_mm_alignr_epi8(_mm_castpd_si128(second), _mm_castpd_si128(first), 8));
+  }
+};
+#else
+// SSE2 versions
+template<int Offset>
+struct palign_impl<Offset,Packet4f>
+{
+  static EIGEN_STRONG_INLINE void run(Packet4f& first, const Packet4f& second)
+  {
+    if (Offset==1)
+    {
+      first = _mm_move_ss(first,second);
+      first = _mm_castsi128_ps(_mm_shuffle_epi32(_mm_castps_si128(first),0x39));
+    }
+    else if (Offset==2)
+    {
+      first = _mm_movehl_ps(first,first);
+      first = _mm_movelh_ps(first,second);
+    }
+    else if (Offset==3)
+    {
+      first = _mm_move_ss(first,second);
+      first = _mm_shuffle_ps(first,second,0x93);
+    }
+  }
+};
+
+template<int Offset>
+struct palign_impl<Offset,Packet4i>
+{
+  static EIGEN_STRONG_INLINE void run(Packet4i& first, const Packet4i& second)
+  {
+    if (Offset==1)
+    {
+      first = _mm_castps_si128(_mm_move_ss(_mm_castsi128_ps(first),_mm_castsi128_ps(second)));
+      first = _mm_shuffle_epi32(first,0x39);
+    }
+    else if (Offset==2)
+    {
+      first = _mm_castps_si128(_mm_movehl_ps(_mm_castsi128_ps(first),_mm_castsi128_ps(first)));
+      first = _mm_castps_si128(_mm_movelh_ps(_mm_castsi128_ps(first),_mm_castsi128_ps(second)));
+    }
+    else if (Offset==3)
+    {
+      first = _mm_castps_si128(_mm_move_ss(_mm_castsi128_ps(first),_mm_castsi128_ps(second)));
+      first = _mm_castps_si128(_mm_shuffle_ps(_mm_castsi128_ps(first),_mm_castsi128_ps(second),0x93));
+    }
+  }
+};
+
+template<int Offset>
+struct palign_impl<Offset,Packet2d>
+{
+  static EIGEN_STRONG_INLINE void run(Packet2d& first, const Packet2d& second)
+  {
+    if (Offset==1)
+    {
+      first = _mm_castps_pd(_mm_movehl_ps(_mm_castpd_ps(first),_mm_castpd_ps(first)));
+      first = _mm_castps_pd(_mm_movelh_ps(_mm_castpd_ps(first),_mm_castpd_ps(second)));
+    }
+  }
+};
+#endif
+
+} // end namespace internal
+
+} // end namespace Eigen
+
+#endif // EIGEN_PACKET_MATH_SSE_H
diff --git a/Util/Patches/Eigen3.1.0/SelfadjointMatrixVector.h b/Util/Patches/Eigen3.1.0/SelfadjointMatrixVector.h
new file mode 100644
index 000000000..2d07ddc34
--- /dev/null
+++ b/Util/Patches/Eigen3.1.0/SelfadjointMatrixVector.h
@@ -0,0 +1,289 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra.
+//
+// Copyright (C) 2008-2009 Gael Guennebaud <gael.guennebaud@inria.fr>
+//
+// Eigen is free software; you can redistribute it and/or
+// modify it under the terms of the GNU Lesser General Public
+// License as published by the Free Software Foundation; either
+// version 3 of the License, or (at your option) any later version.
+//
+// Alternatively, you can redistribute it and/or
+// modify it under the terms of the GNU General Public License as
+// published by the Free Software Foundation; either version 2 of
+// the License, or (at your option) any later version.
+//
+// Eigen is distributed in the hope that it will be useful, but WITHOUT ANY
+// WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
+// FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License or the
+// GNU General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public
+// License and a copy of the GNU General Public License along with
+// Eigen. If not, see <http://www.gnu.org/licenses/>.
+
+#ifndef EIGEN_SELFADJOINT_MATRIX_VECTOR_H
+#define EIGEN_SELFADJOINT_MATRIX_VECTOR_H
+
+namespace Eigen { 
+
+namespace internal {
+
+/* Optimized selfadjoint matrix * vector product:
+ * This algorithm processes 2 columns at onces that allows to both reduce
+ * the number of load/stores of the result by a factor 2 and to reduce
+ * the instruction dependency.
+ */
+
+template<typename Scalar, typename Index, int StorageOrder, int UpLo, bool ConjugateLhs, bool ConjugateRhs, int Version=Specialized>
+struct selfadjoint_matrix_vector_product;
+
+template<typename Scalar, typename Index, int StorageOrder, int UpLo, bool ConjugateLhs, bool ConjugateRhs, int Version>
+struct selfadjoint_matrix_vector_product
+
+{
+static EIGEN_DONT_INLINE void run(
+  Index size,
+  const Scalar*  lhs, Index lhsStride,
+  const Scalar* _rhs, Index rhsIncr,
+  Scalar* res,
+  Scalar alpha)
+{
+  typedef typename packet_traits<Scalar>::type Packet;
+  typedef typename NumTraits<Scalar>::Real RealScalar;
+  const Index PacketSize = sizeof(Packet)/sizeof(Scalar);
+
+  enum {
+    IsRowMajor = StorageOrder==RowMajor ? 1 : 0,
+    IsLower = UpLo == Lower ? 1 : 0,
+    FirstTriangular = IsRowMajor == IsLower
+  };
+
+  conj_helper<Scalar,Scalar,NumTraits<Scalar>::IsComplex && EIGEN_LOGICAL_XOR(ConjugateLhs,  IsRowMajor), ConjugateRhs> cj0;
+  conj_helper<Scalar,Scalar,NumTraits<Scalar>::IsComplex && EIGEN_LOGICAL_XOR(ConjugateLhs, !IsRowMajor), ConjugateRhs> cj1;
+  conj_helper<Scalar,Scalar,NumTraits<Scalar>::IsComplex, ConjugateRhs> cjd;
+
+  conj_helper<Packet,Packet,NumTraits<Scalar>::IsComplex && EIGEN_LOGICAL_XOR(ConjugateLhs,  IsRowMajor), ConjugateRhs> pcj0;
+  conj_helper<Packet,Packet,NumTraits<Scalar>::IsComplex && EIGEN_LOGICAL_XOR(ConjugateLhs, !IsRowMajor), ConjugateRhs> pcj1;
+
+  Scalar cjAlpha = ConjugateRhs ? conj(alpha) : alpha;
+
+  // FIXME this copy is now handled outside product_selfadjoint_vector, so it could probably be removed.
+  // if the rhs is not sequentially stored in memory we copy it to a temporary buffer,
+  // this is because we need to extract packets
+  ei_declare_aligned_stack_constructed_variable(Scalar,rhs,size,rhsIncr==1 ? const_cast<Scalar*>(_rhs) : 0);  
+  if (rhsIncr!=1)
+  {
+    const Scalar* it = _rhs;
+    for (Index i=0; i<size; ++i, it+=rhsIncr)
+      rhs[i] = *it;
+  }
+
+  Index bound = (std::max)(Index(0),size-8) & 0xfffffffe;
+  if (FirstTriangular)
+    bound = size - bound;
+
+  for (Index j=FirstTriangular ? bound : 0;
+       j<(FirstTriangular ? size : bound);j+=2)
+  {
+    const Scalar* EIGEN_RESTRICT A0 = lhs + j*lhsStride;
+    const Scalar* EIGEN_RESTRICT A1 = lhs + (j+1)*lhsStride;
+
+    Scalar t0 = cjAlpha * rhs[j];
+    Packet ptmp0 = pset1<Packet>(t0);
+    Scalar t1 = cjAlpha * rhs[j+1];
+    Packet ptmp1 = pset1<Packet>(t1);
+
+    Scalar t2(0);
+    Packet ptmp2 = pset1<Packet>(t2);
+    Scalar t3(0);
+    Packet ptmp3 = pset1<Packet>(t3);
+
+    size_t starti = FirstTriangular ? 0 : j+2;
+    size_t endi   = FirstTriangular ? j : size;
+    size_t alignedStart = (starti) + internal::first_aligned(&res[starti], endi-starti);
+    size_t alignedEnd = alignedStart + ((endi-alignedStart)/(PacketSize))*(PacketSize);
+
+    // TODO make sure this product is a real * complex and that the rhs is properly conjugated if needed
+    res[j]   += cjd.pmul(internal::real(A0[j]), t0);
+    res[j+1] += cjd.pmul(internal::real(A1[j+1]), t1);
+    if(FirstTriangular)
+    {
+      res[j]   += cj0.pmul(A1[j],   t1);
+      t3       += cj1.pmul(A1[j],   rhs[j]);
+    }
+    else
+    {
+      res[j+1] += cj0.pmul(A0[j+1],t0);
+      t2 += cj1.pmul(A0[j+1], rhs[j+1]);
+    }
+
+    for (size_t i=starti; i<alignedStart; ++i)
+    {
+      res[i] += t0 * A0[i] + t1 * A1[i];
+      t2 += conj(A0[i]) * rhs[i];
+      t3 += conj(A1[i]) * rhs[i];
+    }
+    // Yes this an optimization for gcc 4.3 and 4.4 (=> huge speed up)
+    // gcc 4.2 does this optimization automatically.
+    const Scalar* EIGEN_RESTRICT a0It  = A0  + alignedStart;
+    const Scalar* EIGEN_RESTRICT a1It  = A1  + alignedStart;
+    const Scalar* EIGEN_RESTRICT rhsIt = rhs + alignedStart;
+          Scalar* EIGEN_RESTRICT resIt = res + alignedStart;
+    for (size_t i=alignedStart; i<alignedEnd; i+=PacketSize)
+    {
+      Packet A0i = ploadu<Packet>(a0It);  a0It  += PacketSize;
+      Packet A1i = ploadu<Packet>(a1It);  a1It  += PacketSize;
+      Packet Bi  = ploadu<Packet>(rhsIt); rhsIt += PacketSize; // FIXME should be aligned in most cases
+      Packet Xi  = pload <Packet>(resIt);
+
+      Xi    = pcj0.pmadd(A0i,ptmp0, pcj0.pmadd(A1i,ptmp1,Xi));
+      ptmp2 = pcj1.pmadd(A0i,  Bi, ptmp2);
+      ptmp3 = pcj1.pmadd(A1i,  Bi, ptmp3);
+      pstore(resIt,Xi); resIt += PacketSize;
+    }
+    for (size_t i=alignedEnd; i<endi; i++)
+    {
+      res[i] += cj0.pmul(A0[i], t0) + cj0.pmul(A1[i],t1);
+      t2 += cj1.pmul(A0[i], rhs[i]);
+      t3 += cj1.pmul(A1[i], rhs[i]);
+    }
+
+    res[j]   += alpha * (t2 + predux(ptmp2));
+    res[j+1] += alpha * (t3 + predux(ptmp3));
+  }
+  for (Index j=FirstTriangular ? 0 : bound;j<(FirstTriangular ? bound : size);j++)
+  {
+    const Scalar* EIGEN_RESTRICT A0 = lhs + j*lhsStride;
+
+    Scalar t1 = cjAlpha * rhs[j];
+    Scalar t2(0);
+    // TODO make sure this product is a real * complex and that the rhs is properly conjugated if needed
+    res[j] += cjd.pmul(internal::real(A0[j]), t1);
+    for (Index i=FirstTriangular ? 0 : j+1; i<(FirstTriangular ? j : size); i++)
+    {
+      res[i] += cj0.pmul(A0[i], t1);
+      t2 += cj1.pmul(A0[i], rhs[i]);
+    }
+    res[j] += alpha * t2;
+  }
+}
+};
+
+} // end namespace internal 
+
+/***************************************************************************
+* Wrapper to product_selfadjoint_vector
+***************************************************************************/
+
+namespace internal {
+template<typename Lhs, int LhsMode, typename Rhs>
+struct traits<SelfadjointProductMatrix<Lhs,LhsMode,false,Rhs,0,true> >
+  : traits<ProductBase<SelfadjointProductMatrix<Lhs,LhsMode,false,Rhs,0,true>, Lhs, Rhs> >
+{};
+}
+
+template<typename Lhs, int LhsMode, typename Rhs>
+struct SelfadjointProductMatrix<Lhs,LhsMode,false,Rhs,0,true>
+  : public ProductBase<SelfadjointProductMatrix<Lhs,LhsMode,false,Rhs,0,true>, Lhs, Rhs >
+{
+  EIGEN_PRODUCT_PUBLIC_INTERFACE(SelfadjointProductMatrix)
+
+  enum {
+    LhsUpLo = LhsMode&(Upper|Lower)
+  };
+
+  SelfadjointProductMatrix(const Lhs& lhs, const Rhs& rhs) : Base(lhs,rhs) {}
+
+  template<typename Dest> void scaleAndAddTo(Dest& dest, Scalar alpha) const
+  {
+    typedef typename Dest::Scalar ResScalar;
+    typedef typename Base::RhsScalar RhsScalar;
+    typedef Map<Matrix<ResScalar,Dynamic,1>, Aligned> MappedDest;
+    
+    eigen_assert(dest.rows()==m_lhs.rows() && dest.cols()==m_rhs.cols());
+
+    typename internal::add_const_on_value_type<ActualLhsType>::type lhs = LhsBlasTraits::extract(m_lhs);
+    typename internal::add_const_on_value_type<ActualRhsType>::type rhs = RhsBlasTraits::extract(m_rhs);
+
+    Scalar actualAlpha = alpha * LhsBlasTraits::extractScalarFactor(m_lhs)
+                               * RhsBlasTraits::extractScalarFactor(m_rhs);
+
+    enum {
+      EvalToDest = (Dest::InnerStrideAtCompileTime==1),
+      UseRhs = (_ActualRhsType::InnerStrideAtCompileTime==1)
+    };
+    
+    internal::gemv_static_vector_if<ResScalar,Dest::SizeAtCompileTime,Dest::MaxSizeAtCompileTime,!EvalToDest> static_dest;
+    internal::gemv_static_vector_if<RhsScalar,_ActualRhsType::SizeAtCompileTime,_ActualRhsType::MaxSizeAtCompileTime,!UseRhs> static_rhs;
+
+    ei_declare_aligned_stack_constructed_variable(ResScalar,actualDestPtr,dest.size(),
+                                                  EvalToDest ? dest.data() : static_dest.data());
+                                                  
+    ei_declare_aligned_stack_constructed_variable(RhsScalar,actualRhsPtr,rhs.size(),
+        UseRhs ? const_cast<RhsScalar*>(rhs.data()) : static_rhs.data());
+    
+    if(!EvalToDest)
+    {
+      #ifdef EIGEN_DENSE_STORAGE_CTOR_PLUGIN
+      int size = dest.size();
+      EIGEN_DENSE_STORAGE_CTOR_PLUGIN
+      #endif
+      MappedDest(actualDestPtr, dest.size()) = dest;
+    }
+      
+    if(!UseRhs)
+    {
+      #ifdef EIGEN_DENSE_STORAGE_CTOR_PLUGIN
+      int size = rhs.size();
+      EIGEN_DENSE_STORAGE_CTOR_PLUGIN
+      #endif
+      Map<typename _ActualRhsType::PlainObject>(actualRhsPtr, rhs.size()) = rhs;
+    }
+      
+      
+    internal::selfadjoint_matrix_vector_product<Scalar, Index, (internal::traits<_ActualLhsType>::Flags&RowMajorBit) ? RowMajor : ColMajor, int(LhsUpLo), bool(LhsBlasTraits::NeedToConjugate), bool(RhsBlasTraits::NeedToConjugate)>::run
+      (
+        lhs.rows(),                             // size
+        &lhs.coeffRef(0,0),  lhs.outerStride(), // lhs info
+        actualRhsPtr, 1,                        // rhs info
+        actualDestPtr,                          // result info
+        actualAlpha                             // scale factor
+      );
+    
+    if(!EvalToDest)
+      dest = MappedDest(actualDestPtr, dest.size());
+  }
+};
+
+namespace internal {
+template<typename Lhs, typename Rhs, int RhsMode>
+struct traits<SelfadjointProductMatrix<Lhs,0,true,Rhs,RhsMode,false> >
+  : traits<ProductBase<SelfadjointProductMatrix<Lhs,0,true,Rhs,RhsMode,false>, Lhs, Rhs> >
+{};
+}
+
+template<typename Lhs, typename Rhs, int RhsMode>
+struct SelfadjointProductMatrix<Lhs,0,true,Rhs,RhsMode,false>
+  : public ProductBase<SelfadjointProductMatrix<Lhs,0,true,Rhs,RhsMode,false>, Lhs, Rhs >
+{
+  EIGEN_PRODUCT_PUBLIC_INTERFACE(SelfadjointProductMatrix)
+
+  enum {
+    RhsUpLo = RhsMode&(Upper|Lower)
+  };
+
+  SelfadjointProductMatrix(const Lhs& lhs, const Rhs& rhs) : Base(lhs,rhs) {}
+
+  template<typename Dest> void scaleAndAddTo(Dest& dest, Scalar alpha) const
+  {
+    // let's simply transpose the product
+    Transpose<Dest> destT(dest);
+    SelfadjointProductMatrix<Transpose<const Rhs>, int(RhsUpLo)==Upper ? Lower : Upper, false,
+                             Transpose<const Lhs>, 0, true>(m_rhs.transpose(), m_lhs.transpose()).scaleAndAddTo(destT, alpha);
+  }
+};
+
+} // end namespace Eigen
+
+#endif // EIGEN_SELFADJOINT_MATRIX_VECTOR_H