Extend ROS2 support Step 3: Rework ROS2 handling

CHANGELOG.md

@@ -14,7 +14,8 @@
  * Fixed issue with disabled environment objects (ISMs) appearing in segmentation and depth images.
  * Added support for parsing offsets from OpenDRIVE using optional offset transforms.
  * Added ad-rss type-stubs for the PythonAPI when building with RSS support
-
+ * ROS2Native: Extended functionality and performance of ROS2 support
+
 ## CARLA 0.9.16
 
  * Added NVIDIA Cosmos Transfer1 integration

LibCarla/cmake/fast_dds/CMakeLists.txt

@@ -1,60 +1,97 @@
 cmake_minimum_required(VERSION 3.5.1)
 project(libcarla_fastdds)
 
-# Install headers.
-
-file(GLOB libcarla_carla_fastdds_headers
-  "${libcarla_source_path}/carla/ros2/*.h"
-  "${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 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_fastdds_headers} DESTINATION include/carla/ros2)
-
+  install(FILES ${libcarla_carla_ros2_public_headers} DESTINATION include/carla/ros2/${dir})
+endforeach()
+
+file(GLOB subdirs RELATIVE "${libcarla_source_path}/carla/ros2/fastdds" "${libcarla_source_path}/carla/ros2/fastdds/*")
+foreach(typedir "msg" "srv")
+  foreach(dir ${subdirs})
+    if(IS_DIRECTORY "${libcarla_source_path}/carla/ros2/fastdds/${dir}/${typedir}")
+      file(GLOB libcarla_carla_ros2_types_${dir}_headers
+      "${libcarla_source_path}/carla/ros2/fastdds/${dir}/${typedir}/*.h"
+      "${libcarla_source_path}/carla/ros2/fastdds/${dir}/${typedir}/*.hpp"
+      "${libcarla_source_path}/carla/ros2/fastdds/${dir}/${typedir}/*.ipp"
+      )
+      install(FILES ${libcarla_carla_ros2_types_${dir}_headers} DESTINATION include/carla/ros2/ros_types/${dir}/${typedir}/)
+    endif()
+  endforeach()
+endforeach()
+
+# Install dependencies for UE4 build
 file(GLOB fast_dds_dependencies "${FASTDDS_LIB_PATH}/*.a")
 install(FILES ${fast_dds_dependencies} DESTINATION lib)
-
-
-file(GLOB libcarla_fastdds_sources
-  "${libcarla_source_path}/carla/ros2/*.cpp"
-  "${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")
+file(GLOB fast_dds_include_dirs "${FASTDDS_INCLUDE_PATH}/*")
+foreach(fast_dds_include_dir ${fast_dds_include_dirs})
+  install(DIRECTORY "${fast_dds_include_dir}" DESTINATION include)
+endforeach()
+
+# Collect the sources
+foreach(ros2_dir
+        "/"
+        "fastdds/carla/ros2/impl/"
+        "publishers/"
+        "services/"
+        "subscribers/"
+        "types/")
+
+  file(GLOB sources "${libcarla_source_path}/carla/ros2/${ros2_dir}*.cpp")
+  list(APPEND libcarla_fastdds_sources ${sources})
+endforeach()
+
+file(GLOB msg_sources "${libcarla_source_path}/carla/ros2/fastdds/*/msg/*.cxx")
+list(APPEND libcarla_fastdds_sources ${msg_sources})
+
+file(GLOB srv_sources "${libcarla_source_path}/carla/ros2/fastdds/*/srv/*.cxx")
+list(APPEND libcarla_fastdds_sources ${srv_sources})
 
 # ==============================================================================
 # Create targets for debug and release in the same build type.
 # ==============================================================================
+set(libcarla_fastdds_include_directories
+    # first the fastdds local folder allowing potential overrides of header files
+    "${libcarla_source_path}/carla/ros2/fastdds"
+    "${BOOST_INCLUDE_PATH}"
+    "${RPCLIB_INCLUDE_PATH}"
+    "${FASTDDS_INCLUDE_PATH}"
+    "${libcarla_source_path}/carla/ros2"
+)
 
 if (LIBCARLA_BUILD_RELEASE)
-  add_library(carla_fastdds STATIC ${libcarla_fastdds_sources})
-
-  target_compile_options(carla_fastdds PRIVATE -fexceptions)
-
+  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}")
+    ${libcarla_fastdds_include_directories}
+  )
+  target_link_directories(carla_fastdds PRIVATE 
+    ${FASTDDS_LIB_PATH}
+  )
   install(TARGETS carla_fastdds DESTINATION lib)
-  set_target_properties(carla_fastdds PROPERTIES COMPILE_FLAGS "${CMAKE_CXX_FLAGS_RELEASE}")
-
+  if(NOT WIN32)
+    set(CMAKE_CXX_FLAGS_RELEASE "-O3 ${CMAKE_CXX_FLAGS_RELEASE}" CACHE STRING "" FORCE)
+  endif()
+  set_target_properties(carla_fastdds PROPERTIES COMPILE_FLAGS
+    "-fexceptions ${CMAKE_CXX_FLAGS_RELEASE}")
+  target_compile_definitions(carla_fastdds PUBLIC 
+    WITH_ROS2 CARLA_SERVER_BUILD)
 endif()
 
 if (LIBCARLA_BUILD_DEBUG)
-
-  add_library(carla_fastdds_debug STATIC ${libcarla_fastdds_sources})
-
-  target_compile_options(carla_fastdds_debug PRIVATE -fexceptions)
-
+  add_library(carla_fastdds_debug STATIC 
+    ${libcarla_fastdds_sources}
+  )
   target_include_directories(carla_fastdds_debug SYSTEM PRIVATE
-      "${BOOST_INCLUDE_PATH}"
-      "${RPCLIB_INCLUDE_PATH}")
+      "${libcarla_fastdds_include_directories}"
+  )
   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)
-
+  set_target_properties(carla_fastdds_debug PROPERTIES COMPILE_FLAGS
+    "-fexceptions ${CMAKE_CXX_FLAGS_DEBUG}")
+  target_compile_definitions(carla_fastdds_debug PUBLIC 
+    WITH_ROS2 CARLA_SERVER_BUILD BOOST_ASIO_ENABLE_BUFFER_DEBUGGING)
 endif()

LibCarla/cmake/server/CMakeLists.txt

@@ -23,8 +23,7 @@ foreach(dir ""
             "rpc/"
             "sensor/" "sensor/data/" "sensor/s11n/"
             "streaming/" "streaming/detail/" "streaming/detail/tcp/" "streaming/low_level/"
-            "multigpu/"
-            "ros2/")
+            "multigpu/")
 
   file(GLOB headers "${libcarla_source_path}/carla/${dir}*.h")
   install(FILES ${headers} DESTINATION include/carla/${dir})

LibCarla/source/carla/Buffer.cpp

@@ -7,6 +7,7 @@ namespace carla {
   void Buffer::ReuseThisBuffer() {
     auto pool = _parent_pool.lock();
     if (pool != nullptr) {
+      log_verbose("Buffer[", static_cast<void*>(_data.get()), ":", _size, "]::ReuseThisBuffer() returning buffer to pool:", pool.get());
       pool->Push(std::move(*this));
     }
   }

LibCarla/source/carla/Buffer.h

@@ -251,9 +251,9 @@ namespace carla {
     /// allocated.
     void reset(size_type size) {
       if (_capacity < size) {
-        log_debug("allocating buffer of", size, "bytes");
         _data = std::make_unique<value_type[]>(size);
         _capacity = size;
+        log_verbose("Buffer[", static_cast<void*>(_data.get()), ":", size, "]::reset() Allocated buffer data (old size: ", _size, ")");
       }
       _size = size;
     }

LibCarla/source/carla/Logging.h

@@ -8,6 +8,7 @@
 
 #include "carla/Platform.h"
 
+#define LIBCARLA_LOG_LEVEL_VERBOSE    1
 #define LIBCARLA_LOG_LEVEL_DEBUG     10
 #define LIBCARLA_LOG_LEVEL_INFO      20
 #define LIBCARLA_LOG_LEVEL_WARNING   30
@@ -62,6 +63,21 @@ namespace logging {
 
 } // namespace logging
 
+#if LIBCARLA_LOG_LEVEL <= LIBCARLA_LOG_LEVEL_VERBOSE
+
+  template <typename ... Args>
+  static inline void log_verbose(Args && ... args) {
+    logging::write_to_stream(std::cout, "VERBOSE:", std::forward<Args>(args) ..., '\n');
+  }
+
+#else
+
+  template <typename ... Args>
+  static inline void log_verbose(Args && ...) {}
+
+#endif
+
+
 #if LIBCARLA_LOG_LEVEL <= LIBCARLA_LOG_LEVEL_DEBUG
 
   template <typename ... Args>

LibCarla/source/carla/geom/Location.h

@@ -6,6 +6,9 @@
 
 #pragma once
 
+#include <ostream>
+#include <sstream>
+
 #include "carla/geom/Vector3D.h"
 #include "carla/geom/Vector3DInt.h"
 #include "carla/geom/Math.h"
@@ -108,3 +111,13 @@ namespace geom {
 
 } // namespace geom
 } // namespace carla
+
+namespace std {
+
+inline std::string to_string(carla::geom::Location const &location) {
+  std::stringstream str;
+  str << location;
+  return str.str();
+}
+
+}  // namespace std