Enable multiple color image encodings

.gitignore

@@ -1,4 +1,6 @@
 **/build
+**/log
+**/install
 
 
 # Prerequisites

.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 = git@github.com:nakai-omer/cuda_image_convert.git

nvblox_ros/CMakeLists.txt

@@ -38,6 +38,8 @@ find_package(libstatistics_collector REQUIRED)
 find_package(message_filters REQUIRED)
 find_package(Threads REQUIRED)
 find_package(nvblox REQUIRED)
+find_package(glog REQUIRED)
+find_package(cuda_image_convert REQUIRED)
 find_package(visualization_msgs REQUIRED)
 
 ########
@@ -57,7 +59,7 @@ add_library(${PROJECT_NAME}_lib SHARED
   src/lib/nvblox_node.cpp
   src/lib/qos.cpp
 )
-target_link_libraries(${PROJECT_NAME}_lib nvblox::nvblox_lib nvblox::nvblox_eigen pthread)
+target_link_libraries(${PROJECT_NAME}_lib nvblox::nvblox_lib nvblox::nvblox_eigen pthread cuda_image_convert)
 ament_target_dependencies(${PROJECT_NAME}_lib
   nvblox
   sensor_msgs
@@ -69,6 +71,7 @@ ament_target_dependencies(${PROJECT_NAME}_lib
   std_srvs
   tf2_ros
   message_filters
+  cuda_image_convert
   visualization_msgs
 )
 target_include_directories(${PROJECT_NAME}_lib PUBLIC

nvblox_ros/include/nvblox_ros/conversions.hpp

@@ -22,9 +22,10 @@
 #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>
 #include <visualization_msgs/msg/marker_array.hpp>
 
-
 namespace nvblox
 {
 

nvblox_ros/package.xml

@@ -17,12 +17,12 @@ license agreement from NVIDIA CORPORATION is strictly prohibited.
   <description>NVBlox ROS2 interface</description>
 
   <maintainer email="holeynikova@nvidia.com">Helen Oleynikova</maintainer>
-  <maintainer email="hemals@nvidia.com">Hemal Shah</maintainer> 
+  <maintainer email="hemals@nvidia.com">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>
 
@@ -37,6 +37,7 @@ license agreement from NVIDIA CORPORATION is strictly prohibited.
   <depend>tf2_ros</depend>
   <depend>message_filters</depend>
   <depend>nvblox</depend>
+  <depend>cuda_image_convert</depend>
 
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>ament_lint_common</test_depend>

nvblox_ros/src/lib/conversions.cpp

@@ -40,14 +40,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;
 }