From e065a7af179144042f59dc648dcc46d656a89ecf Mon Sep 17 00:00:00 2001 From: YuxuanLiuTier4Desktop <619684051@qq.com> Date: Mon, 11 Nov 2024 14:57:06 +0900 Subject: [PATCH 1/5] initialize cuda type adaptation Signed-off-by: YuxuanLiuTier4Desktop <619684051@qq.com> --- CMakeLists.txt | 5 ++ include/accelerator/jpeg_compressor.hpp | 10 ++- include/accelerator/rectifier.hpp | 7 +- include/gpu_imgproc/gpu_imgproc.hpp | 18 +++++ package.xml | 1 + src/accelerator/jpeg_compressor.cpp | 101 ++++++++++++++++++++++++ src/accelerator/rectifier.cpp | 41 ++++++++++ src/gpu_imgproc.cpp | 89 +++++++++++++++++++-- 8 files changed, 265 insertions(+), 7 deletions(-) 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 3a893a8..9274376 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 #ifdef TURBOJPEG_AVAILABLE @@ -24,7 +26,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 @@ -37,6 +42,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_; @@ -51,6 +57,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, int format = TJPF_RGB, int sampling = TJ_420); void setCudaStream(cuda::stream::handle_t &raw_cuda_stream); private: NvJPEGEncoder *encoder_; @@ -75,6 +82,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..3150e18 100644 --- a/include/gpu_imgproc/gpu_imgproc.hpp +++ b/include/gpu_imgproc/gpu_imgproc.hpp @@ -5,8 +5,18 @@ #include "accelerator/rectifier.hpp" #include "accelerator/jpeg_compressor.hpp" +#include "type_adapters/image_container.hpp" +#include "type_adapters/compressed_image_container.hpp" // #include +RCLCPP_USING_CUSTOM_TYPE_AS_ROS_MESSAGE_TYPE( + autoware::type_adaptation::type_adapters::ImageContainer, + sensor_msgs::msg::Image); + +RCLCPP_USING_CUSTOM_TYPE_AS_ROS_MESSAGE_TYPE( + autoware::type_adaptation::type_adapters::CompressedImageContainer, + sensor_msgs::msg::CompressedImage); + namespace gpu_imgproc { class GpuImgProc : public rclcpp::Node { @@ -15,8 +25,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(std::shared_ptr msg); void determineQosCallback(bool do_rectify); #if NPP_AVAILABLE @@ -40,9 +55,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 +68,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 ead8c89..ea8208a 100644 --- a/src/accelerator/jpeg_compressor.cpp +++ b/src/accelerator/jpeg_compressor.cpp @@ -70,6 +70,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 @@ -177,6 +183,94 @@ CompressedImage::UniquePtr JetsonCompressor::compress(const Image &msg, int qual return compressed_msg; } +CompressedImage::UniquePtr JetsonCompressor::compress(const ImageContainer &msg, int quality, int format, int sampling) +{ + stream_ = msg.cuda_stream()->stream(); + 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 + + // 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); + } + + buffer_.emplace(V4L2_PIX_FMT_YUV420M, width, height, 0); + TEST_ERROR(buffer_->allocateMemory() != 0, "NvBuffer allocation failed"); + + encoder_->setCropRect(0, 0, width, height); + } + NppiSize roi = {static_cast(msg.width), static_cast(msg.height)}; + if (format == ImageFormat::RGB) { + TEST_ERROR(nppiRGBToYUV420_8u_C3P3R_Ctx(msg.cuda_mem(), 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(); + + size_t out_buf_size = width * height * 3 / 2; + 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(), @@ -228,6 +322,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 1dbd74a..a4cb216 100644 --- a/src/accelerator/rectifier.cpp +++ b/src/accelerator/rectifier.cpp @@ -253,6 +253,23 @@ 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_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_, + result->cuda_mem(), dst_step_, dst_roi_size, interpolation)); +} #endif #ifdef OPENCV_AVAILABLE @@ -288,6 +305,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 @@ -337,6 +366,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 0b841e9..013b78c 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)); + } + { + 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); @@ -223,6 +240,68 @@ void GpuImgProc::imageCallback(const sensor_msgs::msg::Image::SharedPtr msg) { compressed_msg.wait(); } +void GpuImgProc::gpuImageCallback(std::shared_ptr msg) { + RCLCPP_DEBUG(this->get_logger(), "Received image"); + + std::future rectified_msg; + if (rectifier_active_) { + RCLCPP_DEBUG(this->get_logger(), "Rectifying image"); + rectified_msg = + std::async(std::launch::async, [this, msg]() { + 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. + + // rectified_pub_->publish(std::move(rect_img)); + gpu_rectified_pub_->publish(*rect_img); + rect_compressed_pub_->publish(std::move(rect_comp_img)); + }); + } else { + RCLCPP_DEBUG(this->get_logger(), "Not rectifying image"); + } + + std::future compressed_msg = + std::async(std::launch::async, [this, msg]() { + 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)); + }); + + if (rectifier_active_) { + rectified_msg.wait(); + } + compressed_msg.wait(); +} + + void GpuImgProc::cameraInfoCallback(const sensor_msgs::msg::CameraInfo::SharedPtr msg) { RCLCPP_INFO(this->get_logger(), "Received camera info"); From deca20a11945e9253a5c921fe858ff1d9e656f58 Mon Sep 17 00:00:00 2001 From: Yuxuan Liu Date: Mon, 11 Nov 2024 18:35:58 +0900 Subject: [PATCH 2/5] addapting image container for image processor Signed-off-by: Yuxuan Liu --- include/accelerator/jpeg_compressor.hpp | 2 +- src/accelerator/jpeg_compressor.cpp | 83 +++++++++++++++++-------- src/accelerator/rectifier.cpp | 24 ++++++- src/gpu_imgproc.cpp | 7 +-- 4 files changed, 83 insertions(+), 33 deletions(-) diff --git a/include/accelerator/jpeg_compressor.hpp b/include/accelerator/jpeg_compressor.hpp index 9274376..e853bf7 100644 --- a/include/accelerator/jpeg_compressor.hpp +++ b/include/accelerator/jpeg_compressor.hpp @@ -57,7 +57,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, int format = TJPF_RGB, int sampling = TJ_420); + 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_; diff --git a/src/accelerator/jpeg_compressor.cpp b/src/accelerator/jpeg_compressor.cpp index ea8208a..b4ce76e 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" @@ -132,8 +135,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), @@ -153,6 +170,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, @@ -183,19 +201,35 @@ CompressedImage::UniquePtr JetsonCompressor::compress(const Image &msg, int qual return compressed_msg; } -CompressedImage::UniquePtr JetsonCompressor::compress(const ImageContainer &msg, int quality, int format, int sampling) +CompressedImage::UniquePtr JetsonCompressor::compress(const ImageContainer &msg, int quality, ImageFormat format) { - stream_ = msg.cuda_stream()->stream(); + 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->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_); @@ -204,32 +238,30 @@ CompressedImage::UniquePtr JetsonCompressor::compress(const ImageContainer &msg, 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 - - // 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); - } - 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 + } } - NppiSize roi = {static_cast(msg.width), static_cast(msg.height)}; + + 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(msg.cuda_mem(), dev_image_step_bytes_, + 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"); @@ -255,7 +287,6 @@ CompressedImage::UniquePtr JetsonCompressor::compress(const ImageContainer &msg, "memory copy from Device to Host for V plane failed"); stream_.synchronize(); - size_t out_buf_size = width * height * 3 / 2; unsigned char * out_data = new unsigned char[out_buf_size]; TEST_ERROR( diff --git a/src/accelerator/rectifier.cpp b/src/accelerator/rectifier.cpp index a4cb216..57ea9d7 100644 --- a/src/accelerator/rectifier.cpp +++ b/src/accelerator/rectifier.cpp @@ -265,10 +265,30 @@ ImageContainerUniquePtr NPPRectifier::rectify(const ImageContainer &msg) { 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( - src_, src_size, src_step_, src_roi, + 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(), dst_step_, dst_roi_size, interpolation)); + result->cuda_mem(), msg.step(), dst_roi_size, interpolation)); + return result; } #endif diff --git a/src/gpu_imgproc.cpp b/src/gpu_imgproc.cpp index 013b78c..be7f788 100644 --- a/src/gpu_imgproc.cpp +++ b/src/gpu_imgproc.cpp @@ -16,7 +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); + 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()); @@ -146,7 +146,7 @@ void GpuImgProc::determineQosCallback(bool do_rectify) { 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)); } @@ -275,8 +275,7 @@ void GpuImgProc::gpuImageCallback(std::shared_ptr msg) { // 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)); - gpu_rectified_pub_->publish(*rect_img); + gpu_rectified_pub_->publish(std::move(rect_img)); rect_compressed_pub_->publish(std::move(rect_comp_img)); }); } else { From 130fbc59b2aa2959241333c9cb93806585d39860 Mon Sep 17 00:00:00 2001 From: Yuxuan Liu Date: Tue, 12 Nov 2024 18:12:13 +0900 Subject: [PATCH 3/5] rclcpp usingcustom type now moved to adapter types Signed-off-by: Yuxuan Liu --- include/gpu_imgproc/gpu_imgproc.hpp | 8 -------- 1 file changed, 8 deletions(-) diff --git a/include/gpu_imgproc/gpu_imgproc.hpp b/include/gpu_imgproc/gpu_imgproc.hpp index 3150e18..9d84ba9 100644 --- a/include/gpu_imgproc/gpu_imgproc.hpp +++ b/include/gpu_imgproc/gpu_imgproc.hpp @@ -9,14 +9,6 @@ #include "type_adapters/compressed_image_container.hpp" // #include -RCLCPP_USING_CUSTOM_TYPE_AS_ROS_MESSAGE_TYPE( - autoware::type_adaptation::type_adapters::ImageContainer, - sensor_msgs::msg::Image); - -RCLCPP_USING_CUSTOM_TYPE_AS_ROS_MESSAGE_TYPE( - autoware::type_adaptation::type_adapters::CompressedImageContainer, - sensor_msgs::msg::CompressedImage); - namespace gpu_imgproc { class GpuImgProc : public rclcpp::Node { From db40dabf1821024a655223b5ea123a7c11caaef3 Mon Sep 17 00:00:00 2001 From: Yuxuan Liu <619684051@qq.com> Date: Mon, 18 Nov 2024 16:44:12 +0900 Subject: [PATCH 4/5] let the function argument to be constant Signed-off-by: Yuxuan Liu <619684051@qq.com> --- include/gpu_imgproc/gpu_imgproc.hpp | 2 +- src/gpu_imgproc.cpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/include/gpu_imgproc/gpu_imgproc.hpp b/include/gpu_imgproc/gpu_imgproc.hpp index 9d84ba9..50adc11 100644 --- a/include/gpu_imgproc/gpu_imgproc.hpp +++ b/include/gpu_imgproc/gpu_imgproc.hpp @@ -23,7 +23,7 @@ class GpuImgProc : public rclcpp::Node { 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(std::shared_ptr msg); + void gpuImageCallback(const std::shared_ptr msg); void determineQosCallback(bool do_rectify); #if NPP_AVAILABLE diff --git a/src/gpu_imgproc.cpp b/src/gpu_imgproc.cpp index be7f788..5b2ead3 100644 --- a/src/gpu_imgproc.cpp +++ b/src/gpu_imgproc.cpp @@ -240,7 +240,7 @@ void GpuImgProc::imageCallback(const sensor_msgs::msg::Image::SharedPtr msg) { compressed_msg.wait(); } -void GpuImgProc::gpuImageCallback(std::shared_ptr msg) { +void GpuImgProc::gpuImageCallback(const std::shared_ptr msg) { RCLCPP_DEBUG(this->get_logger(), "Received image"); std::future rectified_msg; From cbef8eba870b587e8a314a18eb258aaa8a503c34 Mon Sep 17 00:00:00 2001 From: YuxuanLiuTier4Desktop <619684051@qq.com> Date: Fri, 6 Dec 2024 16:39:38 +0900 Subject: [PATCH 5/5] clean up the async functions Signed-off-by: YuxuanLiuTier4Desktop <619684051@qq.com> --- src/gpu_imgproc.cpp | 151 +++++++++++++++++++------------------------- 1 file changed, 64 insertions(+), 87 deletions(-) diff --git a/src/gpu_imgproc.cpp b/src/gpu_imgproc.cpp index e5c1e29..a43c1ab 100644 --- a/src/gpu_imgproc.cpp +++ b/src/gpu_imgproc.cpp @@ -190,124 +190,101 @@ 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. - - // compressed_pub_->publish(std::move(comp_img)); - compressed_pub_->publish(*comp_img); - }); + 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. - if (rectifier_active_) { - rectified_msg.wait(); - } - compressed_msg.wait(); + // 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"); - std::future rectified_msg; if (rectifier_active_) { RCLCPP_DEBUG(this->get_logger(), "Rectifying image"); - rectified_msg = - std::async(std::launch::async, [this, msg]() { - ImageContainerUniquePtr rect_img; - sensor_msgs::msg::CompressedImage::UniquePtr rect_comp_img; - if (false) { + 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_); + } 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_); + } 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_); + } 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_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"); } - std::future compressed_msg = - std::async(std::launch::async, [this, msg]() { - 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. + 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)); - }); - - if (rectifier_active_) { - rectified_msg.wait(); - } - compressed_msg.wait(); + compressed_pub_->publish(std::move(comp_img)); }