Add support for multiple color image encodings

Author: nakai-omer

.gitmodules

@@ -2,3 +2,6 @@
 	path = nvblox
 	url = https://github.com/nvidia-isaac/nvblox.git
 	branch = public
+[submodule "cuda_image_convert"]
+	path = cuda_image_convert
+	url = [email protected]:nakai-omer/cuda_image_convert.git

cuda_image_convert

@@ -39,6 +39,7 @@ find_package(message_filters REQUIRED)
 find_package(Threads REQUIRED)
 find_package(nvblox REQUIRED)
 find_package(glog REQUIRED)
+find_package(cuda_image_convert REQUIRED)
 
 ########
 # CUDA #
@@ -55,7 +56,7 @@ add_library(${PROJECT_NAME}_lib SHARED
   src/lib/transformer.cpp
   src/lib/nvblox_node.cpp
 )
-target_link_libraries(${PROJECT_NAME}_lib nvblox::nvblox_lib nvblox::nvblox_eigen)
+target_link_libraries(${PROJECT_NAME}_lib nvblox::nvblox_lib nvblox::nvblox_eigen cuda_image_convert)
 ament_target_dependencies(${PROJECT_NAME}_lib
   nvblox
   sensor_msgs
@@ -67,6 +68,7 @@ ament_target_dependencies(${PROJECT_NAME}_lib
   tf2_ros
   tf2_eigen
   message_filters
+  cuda_image_convert
 )
 target_include_directories(${PROJECT_NAME}_lib PUBLIC
   $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>

nvblox_ros/CMakeLists.txt

@@ -21,6 +21,8 @@
 #include <sensor_msgs/msg/camera_info.hpp>
 #include <sensor_msgs/msg/image.hpp>
 #include <sensor_msgs/msg/point_cloud2.hpp>
+#include <cudaColorspace.h>
+#include <cudaMappedMemory.h>
 
 namespace nvblox
 {

nvblox_ros/include/nvblox_ros/conversions.hpp

@@ -17,12 +17,12 @@ license agreement from NVIDIA CORPORATION is strictly prohibited.
   <description>NVBlox ROS2 interface</description>
 
   <maintainer email="[email protected]">Helen Oleynikova</maintainer>
-  <maintainer email="[email protected]">Hemal Shah</maintainer> 
+  <maintainer email="[email protected]">Hemal Shah</maintainer>
   <license>NVIDIA Isaac ROS Software License</license>
-  <url type="website">https://developer.nvidia.com/isaac-ros-gems/</url>   
+  <url type="website">https://developer.nvidia.com/isaac-ros-gems/</url>
   <author>Helen Oleynikova</author>
   <author>Alexander Millane</author>
-  
+
   <buildtool_depend>ament_cmake</buildtool_depend>
   <buildtool_depend>ament_cmake_auto</buildtool_depend>
 

nvblox_ros/package.xml

@@ -37,14 +37,35 @@ bool RosConverter::colorImageFromImageMessage(
 {
   CHECK_NOTNULL(color_image);
 
-  // First check if we actually have a valid image here.
+  void* final_image = NULL;
+
   if (image_msg->encoding != "rgb8") {
-    return false;
+    if(!cudaAllocMapped(&final_image, image_msg->width, image_msg->height, IMAGE_RGB8)) {
+      return false;
+    }
+
+    auto input_format = imageFormatFromStr(image_msg->encoding.c_str());
+    void* inputCPU = NULL;
+    void* inputGPU = NULL;
+    auto input_size = imageFormatSize(input_format, image_msg->width, image_msg->height);
+
+    if (!cudaAllocMapped((void**)&inputCPU, (void**)&inputGPU, input_size)) {
+      return false;
+    }
+
+    memcpy(inputCPU, image_msg->data.data(), input_size);
+  
+    if(CUDA_FAILED(cudaConvertColor(inputGPU, input_format, final_image, IMAGE_RGB8, image_msg->width, image_msg->height)))	{
+			return false;
+		}
+  } 
+  else {
+    final_image = const_cast<uint8_t*>(&image_msg->data[0]);
   }
 
   color_image->populateFromBuffer(
     image_msg->height, image_msg->width,
-    reinterpret_cast<const Color *>(&image_msg->data[0]), MemoryType::kDevice);
+    reinterpret_cast<const Color *>(final_image), MemoryType::kDevice);
 
   return true;
 }