diff --git a/CMakeLists.txt b/CMakeLists.txt index 241e472..f508063 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -25,6 +25,7 @@ endif() find_package(CUDA) find_package(NVJPEG) +find_package(autoware_type_adapters) find_library(CUDART_LIBRARY cudart ${CMAKE_CUDA_IMPLICIT_LINK_DIRECTORIES}) find_library(CULIBOS culibos ${CMAKE_CUDA_IMPLICIT_LINK_DIRECTORIES}) if (${NVJPEG_FOUND} AND CUDART_LIBRARY AND CULIBOS) @@ -150,6 +151,10 @@ if (JETSON) rectifier jpeg_compressor ) + + ament_target_dependencies(accelerated_image_processor + autoware_type_adapters + ) rclcpp_components_register_node(accelerated_image_processor PLUGIN "gpu_imgproc::GpuImgProc" diff --git a/include/accelerator/jpeg_compressor.hpp b/include/accelerator/jpeg_compressor.hpp index 0011903..81a984f 100644 --- a/include/accelerator/jpeg_compressor.hpp +++ b/include/accelerator/jpeg_compressor.hpp @@ -2,6 +2,8 @@ #include #include +#include "type_adapters/image_container.hpp" +#include "type_adapters/compressed_image_container.hpp" #include // This needs to be included before other CUDA headers in some environments @@ -29,7 +31,10 @@ class NvJPEGEncoder; namespace JpegCompressor { using Image = sensor_msgs::msg::Image; using CompressedImage = sensor_msgs::msg::CompressedImage; - +using ImageContainer = autoware::type_adaptation::type_adapters::ImageContainer; +using CompressedImageContainer = autoware::type_adaptation::type_adapters::CompressedImageContainer; +using ImageContainerUniquePtr = autoware::type_adaptation::type_adapters::ImageContainerUniquePtr; +using CompressedImageContainerUniquePtr = autoware::type_adaptation::type_adapters::CompressedImageContainerUniquePtr; enum class ImageFormat { RGB, BGR @@ -42,6 +47,7 @@ class CPUCompressor { ~CPUCompressor(); CompressedImage::UniquePtr compress(const Image &msg, int quality = 90, int format = TJPF_RGB, int sampling = TJ_420); + CompressedImage::UniquePtr compress(const ImageContainer &msg, int quality = 90, int format = TJPF_RGB, int sampling = TJ_420); private: tjhandle handle_; unsigned char *jpegBuf_; @@ -56,6 +62,7 @@ class JetsonCompressor { ~JetsonCompressor(); CompressedImage::UniquePtr compress(const Image &msg, int quality = 90, ImageFormat format = ImageFormat::RGB); + CompressedImage::UniquePtr compress(const ImageContainer &msg, int quality = 90, ImageFormat format = ImageFormat::RGB); void setCudaStream(cuda::stream::handle_t &raw_cuda_stream); private: NvJPEGEncoder *encoder_; @@ -80,6 +87,7 @@ class NVJPEGCompressor { ~NVJPEGCompressor(); CompressedImage::UniquePtr compress(const Image &msg, int quality = 90, ImageFormat format = ImageFormat::RGB); + CompressedImage::UniquePtr compress(const ImageContainer &msg, int quality = 90, ImageFormat format = ImageFormat::RGB); void setCudaStream(const cudaStream_t &raw_cuda_stream); private: diff --git a/include/accelerator/rectifier.hpp b/include/accelerator/rectifier.hpp index c74d4f0..8d2e81b 100644 --- a/include/accelerator/rectifier.hpp +++ b/include/accelerator/rectifier.hpp @@ -2,7 +2,7 @@ #include #include - +#include "type_adapters/image_container.hpp" #ifdef OPENCV_AVAILABLE #include #endif @@ -16,6 +16,8 @@ using CameraInfo = sensor_msgs::msg::CameraInfo; using Image = sensor_msgs::msg::Image; +using ImageContainer = autoware::type_adaptation::type_adapters::ImageContainer; +using ImageContainerUniquePtr = autoware::type_adaptation::type_adapters::ImageContainerUniquePtr; namespace Rectifier { @@ -44,6 +46,7 @@ class NPPRectifier { cudaStream_t& GetCudaStream() {return stream_;} Image::UniquePtr rectify(const Image &msg); + ImageContainerUniquePtr rectify(const ImageContainer &msg); private: Npp32f *pxl_map_x_; Npp32f *pxl_map_y_; @@ -66,6 +69,7 @@ class OpenCVRectifierCPU { ~OpenCVRectifierCPU(); Image::UniquePtr rectify(const Image &msg); + ImageContainerUniquePtr rectify(const ImageContainer &msg); private: cv::Mat map_x_; cv::Mat map_y_; @@ -83,6 +87,7 @@ class OpenCVRectifierGPU { ~OpenCVRectifierGPU(); Image::UniquePtr rectify(const Image &msg); + ImageContainerUniquePtr rectify(const ImageContainer &msg); private: cv::cuda::GpuMat map_x_; cv::cuda::GpuMat map_y_; diff --git a/include/gpu_imgproc/gpu_imgproc.hpp b/include/gpu_imgproc/gpu_imgproc.hpp index 335ff07..50adc11 100644 --- a/include/gpu_imgproc/gpu_imgproc.hpp +++ b/include/gpu_imgproc/gpu_imgproc.hpp @@ -5,6 +5,8 @@ #include "accelerator/rectifier.hpp" #include "accelerator/jpeg_compressor.hpp" +#include "type_adapters/image_container.hpp" +#include "type_adapters/compressed_image_container.hpp" // #include namespace gpu_imgproc { @@ -15,8 +17,13 @@ class GpuImgProc : public rclcpp::Node { virtual ~GpuImgProc(); private: + using ImageContainer = autoware::type_adaptation::type_adapters::ImageContainer; + using CompressedImageContainer = autoware::type_adaptation::type_adapters::CompressedImageContainer; + using ImageContainerUniquePtr = autoware::type_adaptation::type_adapters::ImageContainerUniquePtr; + using CompressedImageContainerUniquePtr = autoware::type_adaptation::type_adapters::CompressedImageContainerUniquePtr; void imageCallback(const sensor_msgs::msg::Image::SharedPtr msg); void cameraInfoCallback(const sensor_msgs::msg::CameraInfo::SharedPtr msg); + void gpuImageCallback(const std::shared_ptr msg); void determineQosCallback(bool do_rectify); #if NPP_AVAILABLE @@ -40,9 +47,11 @@ class GpuImgProc : public rclcpp::Node { #endif rclcpp::Subscription::SharedPtr img_sub_; + rclcpp::Subscription::SharedPtr gpu_image_sub_; rclcpp::Subscription::SharedPtr info_sub_; rclcpp::Publisher::SharedPtr rectified_pub_; + rclcpp::Publisher::SharedPtr gpu_rectified_pub_; rclcpp::Publisher::SharedPtr compressed_pub_; rclcpp::Publisher::SharedPtr rect_compressed_pub_; @@ -51,6 +60,7 @@ class GpuImgProc : public rclcpp::Node { Rectifier::Implementation rectifier_impl_; Rectifier::MappingImpl mapping_impl_; bool rectifier_active_; + bool type_adaptation_active_; double alpha_; int32_t jpeg_quality_; }; diff --git a/package.xml b/package.xml index 3fd40e0..19f0583 100644 --- a/package.xml +++ b/package.xml @@ -18,6 +18,7 @@ cv_bridge libturbojpeg image_geometry + autoware_type_adapters ament_cmake_gtest ament_lint_auto diff --git a/src/accelerator/jpeg_compressor.cpp b/src/accelerator/jpeg_compressor.cpp index e0b7ad1..ebf416a 100644 --- a/src/accelerator/jpeg_compressor.cpp +++ b/src/accelerator/jpeg_compressor.cpp @@ -1,7 +1,10 @@ #include #include - #include +#include +#include + +#include #include "accelerator/jpeg_compressor.hpp" @@ -71,6 +74,12 @@ CompressedImage::UniquePtr CPUCompressor::compress(const Image &msg, int quality return compressed_msg; } +CompressedImage::UniquePtr CPUCompressor::compress(const ImageContainer &msg, int quality, int format, int sampling) { + sensor_msgs::msg::Image image_msg; + msg.get_sensor_msgs_image(image_msg); + return compress(image_msg, quality, format, sampling); +} + #endif #ifdef JETSON_AVAILABLE @@ -127,8 +136,22 @@ CompressedImage::UniquePtr JetsonCompressor::compress(const Image &msg, int qual TEST_ERROR(buffer_->allocateMemory() != 0, "NvBuffer allocation failed"); encoder_->setCropRect(0, 0, width, height); + + // Synchronize CUDA device to ensure memory allocation is complete + std::this_thread::sleep_for(std::chrono::milliseconds(300)); + cudaError_t cudaStatus = cudaDeviceSynchronize(); + if (cudaStatus != cudaSuccess) { + // Handle error + fprintf(stderr, "cudaDeviceSynchronize failed: %s\n", cudaGetErrorString(cudaStatus)); + // You may want to throw an exception or handle the error appropriately + } } + // RCLCPP_ERROR( + // rclcpp::get_logger("v4l2_camera"), + // "dev_yuv_step_bytes_: %d", dev_yuv_step_bytes_[0] + // ); + TEST_ERROR(cudaMemcpy2DAsync(static_cast(dev_image_), dev_image_step_bytes_, static_cast(img.data()), msg.step, msg.step * sizeof(Npp8u), @@ -148,6 +171,7 @@ CompressedImage::UniquePtr JetsonCompressor::compress(const Image &msg, int qual NvBuffer::NvBufferPlane &plane_y = buffer_->planes[0]; NvBuffer::NvBufferPlane &plane_u = buffer_->planes[1]; NvBuffer::NvBufferPlane &plane_v = buffer_->planes[2]; + TEST_ERROR(cudaMemcpy2DAsync(plane_y.data, plane_y.fmt.stride, dev_yuv_[0], dev_yuv_step_bytes_[0], width, height, cudaMemcpyDeviceToHost, stream_.handle()) != cudaSuccess, @@ -178,6 +202,107 @@ CompressedImage::UniquePtr JetsonCompressor::compress(const Image &msg, int qual return compressed_msg; } +CompressedImage::UniquePtr JetsonCompressor::compress(const ImageContainer &msg, int quality, ImageFormat format) +{ + setCudaStream(msg.cuda_stream()->stream()); + // Fill elements of nppStreamContext + { + npp_stream_context_.hStream = stream_.handle(); + cudaGetDevice(&npp_stream_context_.nCudaDeviceId); + cudaDeviceProp dev_prop; + cudaGetDeviceProperties(&dev_prop, npp_stream_context_.nCudaDeviceId); + npp_stream_context_.nMultiProcessorCount = dev_prop.multiProcessorCount; + npp_stream_context_.nMaxThreadsPerMultiProcessor = dev_prop.maxThreadsPerMultiProcessor; + npp_stream_context_.nMaxThreadsPerBlock = dev_prop.maxThreadsPerBlock; + npp_stream_context_.nSharedMemPerBlock = dev_prop.sharedMemPerBlock; + cudaDeviceGetAttribute(&npp_stream_context_.nCudaDevAttrComputeCapabilityMajor, + cudaDevAttrComputeCapabilityMajor, npp_stream_context_.nCudaDeviceId); + cudaDeviceGetAttribute(&npp_stream_context_.nCudaDevAttrComputeCapabilityMinor, + cudaDevAttrComputeCapabilityMinor, npp_stream_context_.nCudaDeviceId); + cudaStreamGetFlags(npp_stream_context_.hStream, &npp_stream_context_.nStreamFlags); + } + + int width = msg.width(); + int height = msg.height(); + // const auto &img = msg.data; + size_t out_buf_size = width * height * 3 / 2; + + CompressedImage::UniquePtr compressed_msg = std::make_unique(); + compressed_msg->header = msg.header(); + compressed_msg->format = "jpeg"; + + if (image_size < out_buf_size) { + // Allocate Npp8u buffers + dev_image_ = nppiMalloc_8u_C3(width, height, &dev_image_step_bytes_); + image_size = out_buf_size; + + dev_yuv_[0] = nppiMalloc_8u_C1(width, height, &dev_yuv_step_bytes_[0]); // Y + dev_yuv_[1] = nppiMalloc_8u_C1(width/2, height/2, &dev_yuv_step_bytes_[1]); // U + dev_yuv_[2] = nppiMalloc_8u_C1(width/2, height/2, &dev_yuv_step_bytes_[2]); // V + buffer_.emplace(V4L2_PIX_FMT_YUV420M, width, height, 0); + TEST_ERROR(buffer_->allocateMemory() != 0, "NvBuffer allocation failed"); + + encoder_->setCropRect(0, 0, width, height); + + // Synchronize CUDA device to ensure memory allocation is complete + std::this_thread::sleep_for(std::chrono::milliseconds(300)); + cudaError_t cudaStatus = cudaDeviceSynchronize(); + if (cudaStatus != cudaSuccess) { + // Handle error + fprintf(stderr, "cudaDeviceSynchronize failed: %s\n", cudaGetErrorString(cudaStatus)); + // You may want to throw an exception or handle the error appropriately + } + } + + TEST_ERROR(cudaMemcpy2DAsync(static_cast(dev_image_), dev_image_step_bytes_, + static_cast(msg.cuda_mem()), msg.step(), + msg.width() * sizeof(Npp8u) * 3, + msg.height(), cudaMemcpyDeviceToDevice, stream_.handle()) != cudaSuccess, + "2D memory allocation failed"); + + NppiSize roi = {static_cast(msg.width()), static_cast(msg.height())}; + if (format == ImageFormat::RGB) { + TEST_ERROR(nppiRGBToYUV420_8u_C3P3R_Ctx(dev_image_, dev_image_step_bytes_, + dev_yuv_.data(), dev_yuv_step_bytes_.data(), roi, + npp_stream_context_) != NPP_SUCCESS, + "failed to convert rgb8 to yuv420"); + } else { + // XXX: need to BGR -> RGB + std::cerr << "not supported" << std::endl; + } + + NvBuffer::NvBufferPlane &plane_y = buffer_->planes[0]; + NvBuffer::NvBufferPlane &plane_u = buffer_->planes[1]; + NvBuffer::NvBufferPlane &plane_v = buffer_->planes[2]; + TEST_ERROR(cudaMemcpy2DAsync(plane_y.data, plane_y.fmt.stride, + dev_yuv_[0], dev_yuv_step_bytes_[0], width, height, + cudaMemcpyDeviceToHost, stream_.handle()) != cudaSuccess, + "memory copy from Device to Host for Y plane failed"); + TEST_ERROR(cudaMemcpy2DAsync(plane_u.data, plane_u.fmt.stride, + dev_yuv_[1], dev_yuv_step_bytes_[1], width/2, height/2, + cudaMemcpyDeviceToHost, stream_.handle()) != cudaSuccess, + "memory copy from Device to Host for U plane failed"); + TEST_ERROR(cudaMemcpy2DAsync(plane_v.data, plane_v.fmt.stride, + dev_yuv_[2], dev_yuv_step_bytes_[2], width/2, height/2, + cudaMemcpyDeviceToHost, stream_.handle()) != cudaSuccess, + "memory copy from Device to Host for V plane failed"); + stream_.synchronize(); + + unsigned char * out_data = new unsigned char[out_buf_size]; + + TEST_ERROR( + encoder_->encodeFromBuffer(buffer_.value(), JCS_YCbCr, &out_data, + out_buf_size, quality), + "NvJpeg Encoder Error"); + + compressed_msg->data.resize(static_cast(out_buf_size / sizeof(uint8_t))); + memcpy(compressed_msg->data.data(), out_data, out_buf_size); + + delete[] out_data; + out_data = nullptr; + return compressed_msg; +} + void JetsonCompressor::setCudaStream(cuda::stream::handle_t &raw_cuda_stream) { stream_ = cuda::stream::wrap(cuda::device::current::get().id(), cuda::context::current::get().handle(), @@ -236,6 +361,13 @@ CompressedImage::UniquePtr NVJPEGCompressor::compress(const Image &msg, int qual return compressed_msg; } +CompressedImage::UniquePtr NVJPEGCompressor::compress(const ImageContainer &msg, int quality, ImageFormat format) +{ + sensor_msgs::msg::Image image_msg; + msg.get_sensor_msgs_image(image_msg); + return compress(image_msg, quality, format); +} + void NVJPEGCompressor::setNVImage(const Image &msg) { unsigned char *p = nullptr; CHECK_CUDA(cudaMallocAsync((void **)&p, msg.data.size(), stream_)); diff --git a/src/accelerator/rectifier.cpp b/src/accelerator/rectifier.cpp index 36742ee..59e5723 100644 --- a/src/accelerator/rectifier.cpp +++ b/src/accelerator/rectifier.cpp @@ -255,6 +255,43 @@ Image::UniquePtr NPPRectifier::rectify(const Image &msg) { return result; } +ImageContainerUniquePtr NPPRectifier::rectify(const ImageContainer &msg) { + nppSetStream(msg.cuda_stream()->stream()); + ImageContainerUniquePtr result = std::make_unique( + msg.header(), msg.height(), msg.width(), msg.encoding(), msg.step(), msg.cuda_stream() + ); + + 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()}; + + NppiInterpolationMode interpolation = NPPI_INTER_LINEAR; + + // CHECK_CUDA(cudaMemcpy2DAsync(src_, src_step_, + // msg.cuda_mem(), msg.step(), msg.width() * 3, + // msg.height(), cudaMemcpyHostToDevice, msg.cuda_stream()->stream())); + + // 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(result->cuda_mem()), + // result->step(), + // static_cast(dst_), + // dst_step_, + // msg.width() * 3 * sizeof(Npp8u), // in byte + // msg.height(), + // cudaMemcpyDeviceToDevice, + // result->cuda_stream()->stream())); + + CHECK_NPP(nppiRemap_8u_C3R( + msg.cuda_mem(), src_size, msg.step(), src_roi, + pxl_map_x_, pxl_map_x_step_, pxl_map_y_, pxl_map_y_step_, + result->cuda_mem(), msg.step(), dst_roi_size, interpolation)); + return result; +} #endif #ifdef OPENCV_AVAILABLE @@ -290,6 +327,18 @@ Image::UniquePtr OpenCVRectifierCPU::rectify(const Image &msg) { return result; } + +ImageContainerUniquePtr OpenCVRectifierCPU::rectify(const ImageContainer &msg) { + RCLCPP_ERROR( + rclcpp::get_logger("v4l2_camera"), + "OpenCVRectifierCPU does not support dealing with image container on GPU"); + + sensor_msgs::msg::Image image_msg; + msg.get_sensor_msgs_image(image_msg); + std::unique_ptr image = rectify(image_msg); + ImageContainerUniquePtr result = std::make_unique(std::move(image)); + return result; +} #endif #ifdef OPENCV_CUDA_AVAILABLE @@ -339,6 +388,18 @@ Image::UniquePtr OpenCVRectifierGPU::rectify(const Image &msg) { return result; } + +ImageContainerUniquePtr OpenCVRectifierGPU::rectify(const ImageContainer &msg) { + RCLCPP_ERROR( + rclcpp::get_logger("v4l2_camera"), + "OpenCVRectifierCPU does not support dealing with image container on GPU"); + + sensor_msgs::msg::Image image_msg; + msg.get_sensor_msgs_image(image_msg); + std::unique_ptr image = rectify(image_msg); + ImageContainerUniquePtr result = std::make_unique(std::move(image)); + return result; +} #endif } // namespace Rectifier diff --git a/src/gpu_imgproc.cpp b/src/gpu_imgproc.cpp index f9e5137..a43c1ab 100644 --- a/src/gpu_imgproc.cpp +++ b/src/gpu_imgproc.cpp @@ -16,6 +16,7 @@ GpuImgProc::GpuImgProc(const rclcpp::NodeOptions & options) alpha_ = this->declare_parameter("alpha", 0.0); jpeg_quality_ = this->declare_parameter("jpeg_quality", 60); bool do_rectify = this->declare_parameter("do_rectify", true); + type_adaptation_active_ = this->declare_parameter("type_adaption_active", true); // RCLCPP_INFO(this->get_logger(), "Subscribing to %s", image_raw_topic.c_str()); // RCLCPP_INFO(this->get_logger(), "Subscribing to %s", camera_info_topic.c_str()); @@ -140,15 +141,31 @@ void GpuImgProc::determineQosCallback(bool do_rectify) { } } + if (type_adaptation_active_) + { + gpu_image_sub_ = this->create_subscription( + img_sub_topic_name, img_qos, std::bind(&GpuImgProc::gpuImageCallback, this, std::placeholders::_1)); + } + else { + img_sub_ = this->create_subscription( + img_sub_topic_name, img_qos, std::bind(&GpuImgProc::imageCallback, this, std::placeholders::_1)); + } compressed_pub_ = this->create_publisher( - "image_raw/compressed", img_qos); - - img_sub_ = this->create_subscription( - img_sub_topic_name, img_qos, std::bind(&GpuImgProc::imageCallback, this, std::placeholders::_1)); + "image_raw/compressed", img_qos); + if (do_rectify) { - rectified_pub_ = this->create_publisher( + if (type_adaptation_active_) + { + gpu_rectified_pub_ = this->create_publisher( + "image_rect", img_qos); + } + else + { + rectified_pub_ = this->create_publisher( "image_rect", img_qos); + } + rect_compressed_pub_ = this->create_publisher( "image_rect/compressed", img_qos); @@ -173,66 +190,104 @@ void GpuImgProc::imageCallback(const sensor_msgs::msg::Image::SharedPtr msg) { "Image encoding (" << msg->encoding << ") is not supported."); } - std::future rectified_msg; + // std::future rectified_msg; if (rectifier_active_) { RCLCPP_DEBUG(this->get_logger(), "Rectifying image"); - rectified_msg = - std::async(std::launch::async, [this, msg, &image_format]() { - sensor_msgs::msg::Image::UniquePtr rect_img; - sensor_msgs::msg::CompressedImage::UniquePtr rect_comp_img; - if (false) { + sensor_msgs::msg::Image::UniquePtr rect_img; + sensor_msgs::msg::CompressedImage::UniquePtr rect_comp_img; + if (false) { #ifdef NPP_AVAILABLE - } else if (rectifier_impl_ == Rectifier::Implementation::NPP) { - rect_img = npp_rectifier_->rectify(*msg); - rect_comp_img = rect_compressor_->compress(*rect_img, jpeg_quality_, image_format); + } else if (rectifier_impl_ == Rectifier::Implementation::NPP) { + rect_img = npp_rectifier_->rectify(*msg); + rect_comp_img = rect_compressor_->compress(*rect_img, jpeg_quality_, image_format); #endif #ifdef OPENCV_AVAILABLE - } else if (rectifier_impl_ == Rectifier::Implementation::OpenCV_CPU) { - rect_img = cv_cpu_rectifier_->rectify(*msg); - rect_comp_img = rect_compressor_->compress(*rect_img, jpeg_quality_, image_format); + } else if (rectifier_impl_ == Rectifier::Implementation::OpenCV_CPU) { + rect_img = cv_cpu_rectifier_->rectify(*msg); + rect_comp_img = rect_compressor_->compress(*rect_img, jpeg_quality_, image_format); #endif #ifdef OPENCV_CUDA_AVAILABLE - } else if (rectifier_impl_ == Rectifier::Implementation::OpenCV_GPU) { - rect_img = cv_gpu_rectifier_->rectify(*msg); - rect_comp_img = rect_compressor_->compress(*rect_img, jpeg_quality_, image_format); + } else if (rectifier_impl_ == Rectifier::Implementation::OpenCV_GPU) { + rect_img = cv_gpu_rectifier_->rectify(*msg); + rect_comp_img = rect_compressor_->compress(*rect_img, jpeg_quality_, image_format); #endif - } else { - RCLCPP_ERROR(this->get_logger(), "Invalid implementation"); - return; - } - // XXX: As of 2023/Nov, publishing the topic via unique_ptr here may cause - // SIGSEGV during cyclonedds process, so the topics are published via passing by value. - // If this SIGSEGV issue will be resolved somehow, it's better to switch back to - // publishing topics via unique_ptr for more efficiency. - - // rectified_pub_->publish(std::move(rect_img)); - // rect_compressed_pub_->publish(std::move(rect_comp_img)); - rectified_pub_->publish(*rect_img); - rect_compressed_pub_->publish(*rect_comp_img); - }); + } else { + RCLCPP_ERROR(this->get_logger(), "Invalid implementation"); + return; + } + // XXX: As of 2023/Nov, publishing the topic via unique_ptr here may cause + // SIGSEGV during cyclonedds process, so the topics are published via passing by value. + // If this SIGSEGV issue will be resolved somehow, it's better to switch back to + // publishing topics via unique_ptr for more efficiency. + + // rectified_pub_->publish(std::move(rect_img)); + // rect_compressed_pub_->publish(std::move(rect_comp_img)); + rectified_pub_->publish(*rect_img); + rect_compressed_pub_->publish(*rect_comp_img); } else { RCLCPP_DEBUG(this->get_logger(), "Not rectifying image"); } - std::future compressed_msg = - std::async(std::launch::async, [this, msg, &image_format]() { - sensor_msgs::msg::CompressedImage::UniquePtr comp_img; - comp_img = raw_compressor_->compress(*msg, jpeg_quality_, image_format); - // XXX: As of 2023/Nov, publishing the topic via unique_ptr here may cause - // SIGSEGV during cyclonedds process, so the topics are published via passing by value. - // If this SIGSEGV issue will be resolved somehow, it's better to switch back to - // publishing topics via unique_ptr for more efficiency. + sensor_msgs::msg::CompressedImage::UniquePtr comp_img; + comp_img = raw_compressor_->compress(*msg, jpeg_quality_, image_format); + // XXX: As of 2023/Nov, publishing the topic via unique_ptr here may cause + // SIGSEGV during cyclonedds process, so the topics are published via passing by value. + // If this SIGSEGV issue will be resolved somehow, it's better to switch back to + // publishing topics via unique_ptr for more efficiency. - // compressed_pub_->publish(std::move(comp_img)); - compressed_pub_->publish(*comp_img); - }); + // compressed_pub_->publish(std::move(comp_img)); + compressed_pub_->publish(*comp_img); +} + +void GpuImgProc::gpuImageCallback(const std::shared_ptr msg) { + RCLCPP_DEBUG(this->get_logger(), "Received image"); if (rectifier_active_) { - rectified_msg.wait(); + RCLCPP_DEBUG(this->get_logger(), "Rectifying image"); + ImageContainerUniquePtr rect_img; + sensor_msgs::msg::CompressedImage::UniquePtr rect_comp_img; + if (false) { +#ifdef NPP_AVAILABLE + } else if (rectifier_impl_ == Rectifier::Implementation::NPP) { + rect_img = npp_rectifier_->rectify(*msg); + rect_comp_img = rect_compressor_->compress(*rect_img, jpeg_quality_); +#endif +#ifdef OPENCV_AVAILABLE + } else if (rectifier_impl_ == Rectifier::Implementation::OpenCV_CPU) { + rect_img = cv_cpu_rectifier_->rectify(*msg); + rect_comp_img = rect_compressor_->compress(*rect_img, jpeg_quality_); +#endif +#ifdef OPENCV_CUDA_AVAILABLE + } else if (rectifier_impl_ == Rectifier::Implementation::OpenCV_GPU) { + rect_img = cv_gpu_rectifier_->rectify(*msg); + rect_comp_img = rect_compressor_->compress(*rect_img, jpeg_quality_); +#endif + } else { + RCLCPP_ERROR(this->get_logger(), "Invalid implementation"); + return; + } + // XXX: As of 2023/Nov, publishing the topic via unique_ptr here may cause + // SIGSEGV during cyclonedds process, so the topics are published via passing by value. + // If this SIGSEGV issue will be resolved somehow, it's better to switch back to + // publishing topics via unique_ptr for more efficiency. + + gpu_rectified_pub_->publish(std::move(rect_img)); + rect_compressed_pub_->publish(std::move(rect_comp_img)); + } else { + RCLCPP_DEBUG(this->get_logger(), "Not rectifying image"); } - compressed_msg.wait(); + + sensor_msgs::msg::CompressedImage::UniquePtr comp_img; + comp_img = raw_compressor_->compress(*msg, jpeg_quality_); + // XXX: As of 2023/Nov, publishing the topic via unique_ptr here may cause + // SIGSEGV during cyclonedds process, so the topics are published via passing by value. + // If this SIGSEGV issue will be resolved somehow, it's better to switch back to + // publishing topics via unique_ptr for more efficiency. + + compressed_pub_->publish(std::move(comp_img)); } + void GpuImgProc::cameraInfoCallback(const sensor_msgs::msg::CameraInfo::SharedPtr msg) { RCLCPP_INFO(this->get_logger(), "Received camera info");