feat: using cuda blackboard

include/accelerator/jpeg_compressor.hpp

@@ -2,6 +2,8 @@
 
 #include <sensor_msgs/msg/image.hpp>
 #include <sensor_msgs/msg/compressed_image.hpp>
+#include "cuda_blackboard/cuda_image.hpp"
+#include "cuda_blackboard/cuda_unique_ptr.hpp"
 #include <string>
 
 // This needs to be included before other CUDA headers in some environments
@@ -29,6 +31,7 @@ class NvJPEGEncoder;
 namespace JpegCompressor {
 using Image = sensor_msgs::msg::Image;
 using CompressedImage = sensor_msgs::msg::CompressedImage;
+using CudaImage = cuda_blackboard::CudaImage;
 
 enum class ImageFormat {
     RGB,
@@ -56,6 +59,7 @@ class JetsonCompressor {
     ~JetsonCompressor();
 
     CompressedImage::UniquePtr compress(const Image &msg, int quality = 90, ImageFormat format = ImageFormat::RGB);
+    CompressedImage::UniquePtr compress(const CudaImage  &msg, int quality = 90, ImageFormat format = ImageFormat::RGB);
     void setCudaStream(cuda::stream::handle_t &raw_cuda_stream);
 private:
     NvJPEGEncoder *encoder_;
@@ -80,6 +84,7 @@ class NVJPEGCompressor {
     ~NVJPEGCompressor();
 
     CompressedImage::UniquePtr compress(const Image &msg, int quality = 90, ImageFormat format = ImageFormat::RGB);
+    CompressedImage::UniquePtr compress(const CudaImage &msg, int quality = 90, ImageFormat format = ImageFormat::RGB);
     void setCudaStream(const cudaStream_t &raw_cuda_stream);
 
 private:

include/accelerator/rectifier.hpp

@@ -2,6 +2,8 @@
 
 #include <sensor_msgs/msg/image.hpp>
 #include <sensor_msgs/msg/camera_info.hpp>
+#include "cuda_blackboard/cuda_image.hpp"
+#include "cuda_blackboard/cuda_unique_ptr.hpp"
 
 #ifdef OPENCV_AVAILABLE
 #include <opencv2/core.hpp>
@@ -31,6 +33,7 @@ enum class MappingImpl {
 };
 
 #if NPP_AVAILABLE
+using CudaImage = cuda_blackboard::CudaImage;
 class NPPRectifier {
 public:
     cudaStream_t stream_;
@@ -43,7 +46,7 @@ class NPPRectifier {
     ~NPPRectifier();
     cudaStream_t& GetCudaStream() {return stream_;}
 
-    Image::UniquePtr rectify(const Image &msg);
+    std::unique_ptr<CudaImage> rectify(const CudaImage  &msg);
 private:
     Npp32f *pxl_map_x_;
     Npp32f *pxl_map_y_;

include/gpu_imgproc/gpu_imgproc.hpp

@@ -2,6 +2,10 @@
 
 #include <optional>
 #include <rclcpp/rclcpp.hpp>
+#include "cuda_blackboard/cuda_adaptation.hpp"
+#include "cuda_blackboard/cuda_blackboard_publisher.hpp"
+#include "cuda_blackboard/cuda_blackboard_subscriber.hpp"
+#include "cuda_blackboard/cuda_image.hpp"
 // #include <rcl_interfaces/msg/parameter.hpp>
 
 #include "accelerator/rectifier.hpp"
@@ -11,13 +15,16 @@
 
 namespace gpu_imgproc {
 
+using CudaImage = cuda_blackboard::CudaImage;
+using CudaBlackboardSubscriber = cuda_blackboard::CudaBlackboardSubscriber<CudaImage>;
+using CudaBlackboardPublisher  = cuda_blackboard::CudaBlackboardPublisher<CudaImage>;
 class GpuImgProc : public rclcpp::Node {
 public:
     explicit GpuImgProc(const rclcpp::NodeOptions & options);
     virtual ~GpuImgProc();
 
 private:
-    void imageCallback(const sensor_msgs::msg::Image::SharedPtr msg);
+    void imageCallback(std::shared_ptr<const CudaImage> msg);
     void cameraInfoCallback(const sensor_msgs::msg::CameraInfo::SharedPtr msg);
     void determineQosCallback(bool do_rectify);
 
@@ -41,10 +48,12 @@ class GpuImgProc : public rclcpp::Node {
     std::shared_ptr<JpegCompressor::CPUCompressor> rect_compressor_;
 #endif
 
-    rclcpp::Subscription<sensor_msgs::msg::Image>::SharedPtr img_sub_;
+    // rclcpp::Subscription<sensor_msgs::msg::Image>::SharedPtr img_sub_;
+    std::shared_ptr<CudaBlackboardSubscriber> img_sub_;
     rclcpp::Subscription<sensor_msgs::msg::CameraInfo>::SharedPtr info_sub_;
 
-    rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr rectified_pub_;
+    // rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr rectified_pub_;
+    std::shared_ptr<CudaBlackboardPublisher> rectified_pub_;
     rclcpp::Publisher<sensor_msgs::msg::CompressedImage>::SharedPtr compressed_pub_;
     rclcpp::Publisher<sensor_msgs::msg::CompressedImage>::SharedPtr rect_compressed_pub_;
 

package.xml

@@ -18,6 +18,7 @@
   <depend>cv_bridge</depend>
   <depend>libturbojpeg</depend>
   <depend>image_geometry</depend>
+  <depend>cuda_blackboard</depend>
 
   <test_depend>ament_cmake_gtest</test_depend>
   <test_depend>ament_lint_auto</test_depend>

src/accelerator/jpeg_compressor.cpp

@@ -94,6 +94,21 @@ JetsonCompressor::~JetsonCompressor() {
     }
 }
 
+CompressedImage::UniquePtr JetsonCompressor::compress(const CudaImage  &msg, int quality, ImageFormat format) {
+    auto ros_image = std::make_unique<sensor_msgs::msg::Image>();
+    ros_image->encoding = msg.encoding;
+    ros_image->height = msg.height;
+    ros_image->width = msg.width;
+    ros_image->step = msg.step;
+    ros_image->is_bigendian = msg.is_bigendian;
+    ros_image->data.resize(msg.height * ros_image->step);
+    cudaMemcpy(
+    ros_image->data.data(), msg.data.get(), ros_image->height * ros_image->step,
+    cudaMemcpyDeviceToHost);
+    return compress(*ros_image, quality, format);
+
+}
+
 CompressedImage::UniquePtr JetsonCompressor::compress(const Image &msg, int quality, ImageFormat format) {
     CompressedImage::UniquePtr compressed_msg = std::make_unique<CompressedImage>();
     compressed_msg->header = msg.header;
@@ -217,6 +232,20 @@ NVJPEGCompressor::~NVJPEGCompressor() {
     CHECK_CUDA(cudaStreamDestroy(stream_));
 }
 
+CompressedImage::UniquePtr NVJPEGCompressor::compress(const CudaImage  &msg, int quality, ImageFormat format) {
+    auto ros_image = std::make_unique<sensor_msgs::msg::Image>();
+    ros_image->encoding = msg.encoding;
+    ros_image->height = msg.height;
+    ros_image->width = msg.width;
+    ros_image->step = msg.step;
+    ros_image->is_bigendian = msg.is_bigendian;
+    ros_image->data.resize(msg.height * ros_image->step);
+    cudaMemcpy(
+    ros_image->data.data(), msg.data.get(), ros_image->height * ros_image->step,
+    cudaMemcpyDeviceToHost);
+    return compress(*ros_image, quality, format);
+}
+
 CompressedImage::UniquePtr NVJPEGCompressor::compress(const Image &msg, int quality, ImageFormat format) {
     CompressedImage::UniquePtr compressed_msg = std::make_unique<CompressedImage>();
     compressed_msg->header = msg.header;

src/accelerator/rectifier.cpp

@@ -213,39 +213,44 @@ NPPRectifier::~NPPRectifier() {
     cudaStreamDestroy(stream_);
 }
 
-Image::UniquePtr NPPRectifier::rectify(const Image &msg) {
+std::unique_ptr<CudaImage> NPPRectifier::rectify(const CudaImage &msg) {
     nppSetStream(stream_);
-    Image::UniquePtr result = std::make_unique<Image>();
+    std::unique_ptr<CudaImage> result = std::make_unique<CudaImage>();
     result->header = msg.header;
     result->height = msg.height;
     result->width = msg.width;
     result->encoding = msg.encoding;
     result->is_bigendian = msg.is_bigendian;
     result->step = msg.step;
 
-    result->data.resize(msg.data.size());
+    result->data = cuda_blackboard::make_unique<uint8_t[]>(msg.height * msg.step * sizeof(uint8_t));
 
     NppiRect src_roi = {0, 0, (int)msg.width, (int)msg.height};
     NppiSize src_size = {(int)msg.width, (int)msg.height};
     NppiSize dst_roi_size = {(int)msg.width, (int)msg.height};
 
-    CHECK_CUDA(cudaMemcpy2DAsync(src_, src_step_, msg.data.data(), msg.step, msg.width * 3, msg.height, cudaMemcpyHostToDevice, stream_));
+    // CHECK_CUDA(cudaMemcpy2DAsync(src_, src_step_, msg.data.data(), msg.step, msg.width * 3, msg.height, cudaMemcpyHostToDevice, stream_));
 
     NppiInterpolationMode interpolation = NPPI_INTER_LINEAR;
 
     CHECK_NPP(nppiRemap_8u_C3R(
-        src_, src_size, src_step_, src_roi,
+        msg.data.get(), src_size, msg.step, src_roi,
         pxl_map_x_, pxl_map_x_step_, pxl_map_y_, pxl_map_y_step_,
-        dst_, dst_step_, dst_roi_size, interpolation));
-
-    CHECK_CUDA(cudaMemcpy2DAsync(static_cast<void*>(result->data.data()),
-                                 result->step,
-                                 static_cast<const void*>(dst_),
-                                 dst_step_,
-                                 msg.width * 3 * sizeof(Npp8u),  // in byte
-                                 msg.height,
-                                 cudaMemcpyDeviceToHost,
-                                 stream_));
+        result->data.get(), msg.step, dst_roi_size, interpolation));
+
+    // CHECK_NPP(nppiRemap_8u_C3R(
+    //     src_, src_size, src_step_, src_roi,
+    //     pxl_map_x_, pxl_map_x_step_, pxl_map_y_, pxl_map_y_step_,
+    //     dst_, dst_step_, dst_roi_size, interpolation));
+
+    // CHECK_CUDA(cudaMemcpy2DAsync(static_cast<void*>(result->data.data()),
+    //                              result->step,
+    //                              static_cast<const void*>(dst_),
+    //                              dst_step_,
+    //                              msg.width * 3 * sizeof(Npp8u),  // in byte
+    //                              msg.height,
+    //                              cudaMemcpyDeviceToHost,
+    //                              stream_));
 
     // cv::Mat image(msg.height, msg.width, CV_8UC3, result->data.data(), result->step);
     // cv::cvtColor(image, image, cv::COLOR_RGB2BGR);