From c78f098ad44a5611ea0f4710830723101992a766 Mon Sep 17 00:00:00 2001 From: YuxuanLiuTier4Desktop <619684051@qq.com> Date: Thu, 29 May 2025 17:21:10 +0900 Subject: [PATCH 1/3] commit cuda black board Signed-off-by: YuxuanLiuTier4Desktop <619684051@qq.com> --- include/accelerator/jpeg_compressor.hpp | 4 + include/accelerator/rectifier.hpp | 5 +- include/gpu_imgproc/gpu_imgproc.hpp | 15 +- package.xml | 1 + src/accelerator/jpeg_compressor.cpp | 15 + src/accelerator/rectifier.cpp | 35 +- src/gpu_imgproc.cpp | 522 ++++++++++++------------ 7 files changed, 321 insertions(+), 276 deletions(-) diff --git a/include/accelerator/jpeg_compressor.hpp b/include/accelerator/jpeg_compressor.hpp index 0011903..c4b0b48 100644 --- a/include/accelerator/jpeg_compressor.hpp +++ b/include/accelerator/jpeg_compressor.hpp @@ -2,6 +2,8 @@ #include #include +#include "cuda_blackboard/cuda_image.hpp" +#include "cuda_blackboard/cuda_unique_ptr.hpp" #include // 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_; diff --git a/include/accelerator/rectifier.hpp b/include/accelerator/rectifier.hpp index c74d4f0..f4194dc 100644 --- a/include/accelerator/rectifier.hpp +++ b/include/accelerator/rectifier.hpp @@ -2,6 +2,8 @@ #include #include +#include "cuda_blackboard/cuda_image.hpp" +#include "cuda_blackboard/cuda_unique_ptr.hpp" #ifdef OPENCV_AVAILABLE #include @@ -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::shared_ptr rectify(const CudaImage &msg); private: Npp32f *pxl_map_x_; Npp32f *pxl_map_y_; diff --git a/include/gpu_imgproc/gpu_imgproc.hpp b/include/gpu_imgproc/gpu_imgproc.hpp index d8c9b18..864501a 100644 --- a/include/gpu_imgproc/gpu_imgproc.hpp +++ b/include/gpu_imgproc/gpu_imgproc.hpp @@ -2,6 +2,10 @@ #include #include +#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 #include "accelerator/rectifier.hpp" @@ -11,13 +15,16 @@ namespace gpu_imgproc { +using CudaImage = cuda_blackboard::CudaImage; +using CudaBlackboardSubscriber = cuda_blackboard::CudaBlackboardSubscriber; +using CudaBlackboardPublisher = cuda_blackboard::CudaBlackboardPublisher; 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 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 rect_compressor_; #endif - rclcpp::Subscription::SharedPtr img_sub_; + // rclcpp::Subscription::SharedPtr img_sub_; + std::shared_ptr img_sub_; rclcpp::Subscription::SharedPtr info_sub_; - rclcpp::Publisher::SharedPtr rectified_pub_; + // rclcpp::Publisher::SharedPtr rectified_pub_; + std::shared_ptr rectified_pub_; rclcpp::Publisher::SharedPtr compressed_pub_; rclcpp::Publisher::SharedPtr rect_compressed_pub_; diff --git a/package.xml b/package.xml index 3fd40e0..387766d 100644 --- a/package.xml +++ b/package.xml @@ -18,6 +18,7 @@ cv_bridge libturbojpeg image_geometry + cuda_blackboard ament_cmake_gtest ament_lint_auto diff --git a/src/accelerator/jpeg_compressor.cpp b/src/accelerator/jpeg_compressor.cpp index f4419c8..da9b744 100644 --- a/src/accelerator/jpeg_compressor.cpp +++ b/src/accelerator/jpeg_compressor.cpp @@ -94,6 +94,21 @@ JetsonCompressor::~JetsonCompressor() { } } +CompressedImage::UniquePtr compress(const CudaImage &msg, int quality = 90, ImageFormat format = ImageFormat::RGB) { + auto ros_image = std::make_unique(); + ros_image->encoding = cuda_msg->encoding; + ros_image->height = cuda_msg->height; + ros_image->width = cuda_msg->width; + ros_image->step = cuda_msg->step; + ros_image->is_bigendian = cuda_msg->is_bigendian; + ros_image->data.resize(cuda_msg->height * ros_image->step); + cudaMemcpy( + ros_image->data.data(), cuda_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(); compressed_msg->header = msg.header; diff --git a/src/accelerator/rectifier.cpp b/src/accelerator/rectifier.cpp index 36742ee..8139fca 100644 --- a/src/accelerator/rectifier.cpp +++ b/src/accelerator/rectifier.cpp @@ -213,9 +213,9 @@ NPPRectifier::~NPPRectifier() { cudaStreamDestroy(stream_); } -Image::UniquePtr NPPRectifier::rectify(const Image &msg) { +std::shared_ptr NPPRectifier::rectify(const CudaImage &msg) { nppSetStream(stream_); - Image::UniquePtr result = std::make_unique(); + std::shared_ptr result = std::make_shared(); result->header = msg.header; result->height = msg.height; result->width = msg.width; @@ -223,29 +223,34 @@ Image::UniquePtr NPPRectifier::rectify(const Image &msg) { result->is_bigendian = msg.is_bigendian; result->step = msg.step; - result->data.resize(msg.data.size()); + result->data = cuda_blackboard::make_unique(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(result->data.data()), - result->step, - static_cast(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(result->data.data()), + // result->step, + // static_cast(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); diff --git a/src/gpu_imgproc.cpp b/src/gpu_imgproc.cpp index 7b60a87..5747d4d 100644 --- a/src/gpu_imgproc.cpp +++ b/src/gpu_imgproc.cpp @@ -2,316 +2,324 @@ #include -namespace gpu_imgproc { +namespace gpu_imgproc +{ GpuImgProc::GpuImgProc(const rclcpp::NodeOptions & options) - : Node("gpu_imgproc", options), rectifier_active_(false) { - RCLCPP_INFO(this->get_logger(), "Initializing node gpu_imgproc"); - - // std::string image_raw_topic = this->declare_parameter("image_raw_topic", "/camera/image_raw"); - // std::string camera_info_topic = this->declare_parameter("camera_info_topic", "/camera/camera_info"); - // std::string image_rect_topic = this->declare_parameter("image_rect_topic", "/camera/image_rect"); - std::string rect_impl = this->declare_parameter("rect_impl", "npp"); - bool use_opencv_map_init = this->declare_parameter("use_opencv_map_init", false); - alpha_ = this->declare_parameter("alpha", 0.0); - jpeg_quality_ = this->declare_parameter("jpeg_quality", 60); - do_rectify_ = this->declare_parameter("do_rectify", true); - // NOTE: too short `max_task_queue_length_` may cause topic drop, while too large one may cause 100% system memory usage under many cameras/high framerate conditions - max_task_queue_length_ = static_cast( - this->declare_parameter("max_task_queue_length", 5)); - - // 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()); - // RCLCPP_INFO(this->get_logger(), "Publishing to %s", image_rect_topic.c_str()); - - std::string available_impls = ""; +: Node("gpu_imgproc", options), rectifier_active_(false) +{ + RCLCPP_INFO(this->get_logger(), "Initializing node gpu_imgproc"); + + // std::string image_raw_topic = this->declare_parameter("image_raw_topic", + // "/camera/image_raw"); std::string camera_info_topic = + // this->declare_parameter("camera_info_topic", "/camera/camera_info"); std::string + // image_rect_topic = this->declare_parameter("image_rect_topic", + // "/camera/image_rect"); + std::string rect_impl = this->declare_parameter("rect_impl", "npp"); + bool use_opencv_map_init = this->declare_parameter("use_opencv_map_init", false); + alpha_ = this->declare_parameter("alpha", 0.0); + jpeg_quality_ = this->declare_parameter("jpeg_quality", 60); + do_rectify_ = this->declare_parameter("do_rectify", true); + // NOTE: too short `max_task_queue_length_` may cause topic drop, while too large one may cause + // 100% system memory usage under many cameras/high framerate conditions + max_task_queue_length_ = + static_cast(this->declare_parameter("max_task_queue_length", 5)); + + // 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()); + // RCLCPP_INFO(this->get_logger(), "Publishing to %s", image_rect_topic.c_str()); + + std::string available_impls = ""; #ifdef NPP_AVAILABLE - available_impls += "npp"; + available_impls += "npp"; #endif #ifdef OPENCV_AVAILABLE - if (available_impls != "") { - available_impls += ", "; - } - available_impls += "opencv_cpu"; + if (available_impls != "") { + available_impls += ", "; + } + available_impls += "opencv_cpu"; #endif #ifdef OPENCV_CUDA_AVAILABLE - if (available_impls != "") { - available_impls += ", "; - } - available_impls += "opencv_gpu"; + if (available_impls != "") { + available_impls += ", "; + } + available_impls += "opencv_gpu"; #endif - if (available_impls == "") { - RCLCPP_ERROR(this->get_logger(), - "No rectification implementations available. Please make sure that at least one of the following libraries is installed:\n" - "- OpenCV\n" - "- OpenCV CUDA\n" - "- NVIDIA Performance Primitives\n"); - return; - } + if (available_impls == "") { + RCLCPP_ERROR( + this->get_logger(), + "No rectification implementations available. Please make sure that at least one of the " + "following libraries is installed:\n" + "- OpenCV\n" + "- OpenCV CUDA\n" + "- NVIDIA Performance Primitives\n"); + return; + } - if (0) { + if (0) { #ifdef NPP_AVAILABLE - } else if (rect_impl == "npp") { - RCLCPP_INFO(this->get_logger(), "Using NPP implementation for rectification"); - rectifier_impl_ = Rectifier::Implementation::NPP; + } else if (rect_impl == "npp") { + RCLCPP_INFO(this->get_logger(), "Using NPP implementation for rectification"); + rectifier_impl_ = Rectifier::Implementation::NPP; #endif #ifdef OPENCV_AVAILABLE - } else if (rect_impl == "opencv_cpu") { - RCLCPP_INFO(this->get_logger(), "Using CPU OpenCV implementation for rectification"); - rectifier_impl_ = Rectifier::Implementation::OpenCV_CPU; + } else if (rect_impl == "opencv_cpu") { + RCLCPP_INFO(this->get_logger(), "Using CPU OpenCV implementation for rectification"); + rectifier_impl_ = Rectifier::Implementation::OpenCV_CPU; #endif #ifdef OPENCV_CUDA_AVAILABLE - } else if (rect_impl == "opencv_gpu") { - RCLCPP_INFO(this->get_logger(), "Using GPU OpenCV implementation for rectification"); - rectifier_impl_ = Rectifier::Implementation::OpenCV_GPU; + } else if (rect_impl == "opencv_gpu") { + RCLCPP_INFO(this->get_logger(), "Using GPU OpenCV implementation for rectification"); + rectifier_impl_ = Rectifier::Implementation::OpenCV_GPU; #endif - } else { - RCLCPP_ERROR(this->get_logger(), "Invalid implementation: %s. Available options: %s", rect_impl.c_str(), available_impls.c_str()); - return; - } + } else { + RCLCPP_ERROR( + this->get_logger(), "Invalid implementation: %s. Available options: %s", rect_impl.c_str(), + available_impls.c_str()); + return; + } - if (use_opencv_map_init) { - RCLCPP_INFO(this->get_logger(), "Using OpenCV map initialization"); - mapping_impl_ = Rectifier::MappingImpl::OpenCV; - } else { - RCLCPP_INFO(this->get_logger(), "Using Non-OpenCV map initialization"); - mapping_impl_ = Rectifier::MappingImpl::NPP; - } + if (use_opencv_map_init) { + RCLCPP_INFO(this->get_logger(), "Using OpenCV map initialization"); + mapping_impl_ = Rectifier::MappingImpl::OpenCV; + } else { + RCLCPP_INFO(this->get_logger(), "Using Non-OpenCV map initialization"); + mapping_impl_ = Rectifier::MappingImpl::NPP; + } #ifdef JETSON_AVAILABLE - raw_compressor_ = std::make_shared("raw_compressor"); - rect_compressor_ = std::make_shared("rect_compressor"); + raw_compressor_ = std::make_shared("raw_compressor"); + rect_compressor_ = std::make_shared("rect_compressor"); #elif NVJPEG_AVAILABLE - raw_compressor_ = std::make_shared(); - rect_compressor_ = std::make_shared(); + raw_compressor_ = std::make_shared(); + rect_compressor_ = std::make_shared(); #elif TURBOJPEG_AVAILABLE - raw_compressor_ = std::make_shared(); - rect_compressor_ = std::make_shared(); + raw_compressor_ = std::make_shared(); + rect_compressor_ = std::make_shared(); #else - RCLCPP_ERROR(this->get_logger(), "No JPEG compressor available"); - return; + RCLCPP_ERROR(this->get_logger(), "No JPEG compressor available"); + return; #endif - // Query QoS using timer to adapt to the case of image publisher starts after this consturctor - qos_request_timer_ = rclcpp::create_timer(this, this->get_clock(), std::chrono::milliseconds(100), - [this]() - {this->determineQosCallback(do_rectify_);}); + // Query QoS using timer to adapt to the case of image publisher starts after this consturctor + qos_request_timer_ = rclcpp::create_timer( + this, this->get_clock(), std::chrono::milliseconds(100), + [this]() { this->determineQosCallback(do_rectify_); }); } -GpuImgProc::~GpuImgProc() { - RCLCPP_INFO(this->get_logger(), "Shutting down node gpu_imgproc"); - if (do_rectify_) { - rectify_task_queue_->stop(); - rectify_worker_->join(); - } - compress_task_queue_->stop(); - compress_worker_->join(); +GpuImgProc::~GpuImgProc() +{ + RCLCPP_INFO(this->get_logger(), "Shutting down node gpu_imgproc"); + if (do_rectify_) { + rectify_task_queue_->stop(); + rectify_worker_->join(); + } + compress_task_queue_->stop(); + compress_worker_->join(); } -void GpuImgProc::determineQosCallback(bool do_rectify) { - // Query QoS to publisher to align the QoS for the topics to be published - auto get_qos = [this](std::string& topic_name, rclcpp::QoS& qos) -> bool { - auto qos_list = this->get_publishers_info_by_topic(topic_name); - if (qos_list.size() < 1) { - RCLCPP_INFO_STREAM(this->get_logger(), "Waiting for" << topic_name << " ..."); - return false; - } else if (qos_list.size() > 1) { - RCLCPP_ERROR(this->get_logger(), - "Multiple publisher for %s are detected. Cannot determine proper QoS", - topic_name.c_str()); - return false; - } else { - RCLCPP_INFO_STREAM(this->get_logger(), - "QoS for " << topic_name << " is acquired."); - qos = qos_list[0].qos_profile(); - return true; - } - }; - - std::string img_sub_topic_name = this->get_node_topics_interface()->resolve_topic_name( - "image_raw", false); - std::string info_sub_topic_name = this->get_node_topics_interface()->resolve_topic_name( - "camera_info", false); - - // Query QoS to publisher to align the QoS for the topics to be published - rclcpp::QoS img_qos(1); - if (!get_qos(img_sub_topic_name, img_qos)) { - // Publisher is not ready yet - return; - } - - rclcpp::QoS info_qos(1); - if (do_rectify) { - if (!get_qos(info_sub_topic_name, info_qos)) { - // Publisher is not ready yet - return; - } +void GpuImgProc::determineQosCallback(bool do_rectify) +{ + // Query QoS to publisher to align the QoS for the topics to be published + auto get_qos = [this](std::string & topic_name, rclcpp::QoS & qos) -> bool { + auto qos_list = this->get_publishers_info_by_topic(topic_name); + if (qos_list.size() < 1) { + RCLCPP_INFO_STREAM(this->get_logger(), "Waiting for" << topic_name << " ..."); + return false; + } else if (qos_list.size() > 1) { + RCLCPP_ERROR( + this->get_logger(), "Multiple publisher for %s are detected. Cannot determine proper QoS", + topic_name.c_str()); + return false; + } else { + RCLCPP_INFO_STREAM(this->get_logger(), "QoS for " << topic_name << " is acquired."); + qos = qos_list[0].qos_profile(); + return true; } + }; - compressed_pub_ = this->create_publisher( - "image_raw/compressed", img_qos); - compress_task_queue_.emplace(max_task_queue_length_); - compress_worker_.emplace(&util::TaskQueue::run, &compress_task_queue_.value()); - - img_sub_ = this->create_subscription( - img_sub_topic_name, img_qos, std::bind(&GpuImgProc::imageCallback, this, std::placeholders::_1)); + std::string img_sub_topic_name = + this->get_node_topics_interface()->resolve_topic_name("image_raw", false); + std::string info_sub_topic_name = + this->get_node_topics_interface()->resolve_topic_name("camera_info", false); - if (do_rectify) { - rectified_pub_ = this->create_publisher( - "image_rect", img_qos); - rect_compressed_pub_ = this->create_publisher( - "image_rect/compressed", img_qos); - rectify_task_queue_.emplace(max_task_queue_length_); - rectify_worker_.emplace(&util::TaskQueue::run, &rectify_task_queue_.value()); + // Query QoS to publisher to align the QoS for the topics to be published + rclcpp::QoS img_qos(1); + if (!get_qos(img_sub_topic_name, img_qos)) { + // Publisher is not ready yet + return; + } - info_sub_ = this->create_subscription( - info_sub_topic_name, info_qos, std::bind(&GpuImgProc::cameraInfoCallback, this, std::placeholders::_1)); + rclcpp::QoS info_qos(1); + if (do_rectify) { + if (!get_qos(info_sub_topic_name, info_qos)) { + // Publisher is not ready yet + return; } - - // Once all queries receive sufficient results, stop the timer - qos_request_timer_->cancel(); + } + + compressed_pub_ = + this->create_publisher("image_raw/compressed", img_qos); + compress_task_queue_.emplace(max_task_queue_length_); + compress_worker_.emplace(&util::TaskQueue::run, &compress_task_queue_.value()); + + img_sub_ = std::make_shared( + *this, "image_raw", false, std::bind(&GpuImgProc::imageCallback, this, std::placeholders::_1)); + + if (do_rectify) { + // rectified_pub_ = this->create_publisher("image_rect", img_qos); + rectified_pub_ = std::make_shared(*this, "image_rect"); + rect_compressed_pub_ = + this->create_publisher("image_rect/compressed", img_qos); + rectify_task_queue_.emplace(max_task_queue_length_); + rectify_worker_.emplace(&util::TaskQueue::run, &rectify_task_queue_.value()); + + info_sub_ = this->create_subscription( + info_sub_topic_name, info_qos, + std::bind(&GpuImgProc::cameraInfoCallback, this, std::placeholders::_1)); + } + + // Once all queries receive sufficient results, stop the timer + qos_request_timer_->cancel(); } -void GpuImgProc::imageCallback(const sensor_msgs::msg::Image::SharedPtr msg) { - RCLCPP_DEBUG(this->get_logger(), "Received image"); - - JpegCompressor::ImageFormat image_format; - if (msg->encoding == "rgb8") { - image_format = JpegCompressor::ImageFormat::RGB; - } else if (msg->encoding == "bgr8") { - image_format = JpegCompressor::ImageFormat::BGR; - } else { - RCLCPP_ERROR_STREAM(this->get_logger(), - "Image encoding (" << msg->encoding << ") is not supported."); - } - - if (rectifier_active_) { - RCLCPP_DEBUG(this->get_logger(), "Rectifying image"); - rectify_task_queue_->addTask([this, msg, image_format]() { - sensor_msgs::msg::Image::UniquePtr rect_img; - sensor_msgs::msg::CompressedImage::UniquePtr rect_comp_img; - if (false) { +void GpuImgProc::imageCallback(std::shared_ptr msg) +{ + RCLCPP_DEBUG(this->get_logger(), "Received image"); + + JpegCompressor::ImageFormat image_format; + if (msg->encoding == "rgb8") { + image_format = JpegCompressor::ImageFormat::RGB; + } else if (msg->encoding == "bgr8") { + image_format = JpegCompressor::ImageFormat::BGR; + } else { + RCLCPP_ERROR_STREAM( + this->get_logger(), "Image encoding (" << msg->encoding << ") is not supported."); + } + + if (rectifier_active_) { + RCLCPP_DEBUG(this->get_logger(), "Rectifying image"); + rectify_task_queue_->addTask([this, msg, image_format]() { + // sensor_msgs::msg::Image::UniquePtr rect_img; + std::shared_ptr 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); -#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::NPP) { + rect_img = npp_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); -#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_DEBUG(this->get_logger(), "Not rectifying image"); - } - - compress_task_queue_->addTask([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); - }); + } 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)); + std::unique_ptr unique_rect_img = std::make_unique(*rect_img); + rectified_pub_->publish(std::move(unique_rect_img)); + rect_compressed_pub_->publish(*rect_comp_img); + }); + } else { + RCLCPP_DEBUG(this->get_logger(), "Not rectifying image"); + } + + compress_task_queue_->addTask([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); + }); } -void GpuImgProc::cameraInfoCallback(const sensor_msgs::msg::CameraInfo::SharedPtr msg) { - RCLCPP_INFO(this->get_logger(), "Received camera info"); +void GpuImgProc::cameraInfoCallback(const sensor_msgs::msg::CameraInfo::SharedPtr msg) +{ + RCLCPP_INFO(this->get_logger(), "Received camera info"); - if (msg->d.size() == 0 || msg->p.size() == 0) { - RCLCPP_ERROR(this->get_logger(), "Camera info message does not contain distortion or projection matrix"); - return; - } + if (msg->d.size() == 0 || msg->p.size() == 0) { + RCLCPP_ERROR( + this->get_logger(), "Camera info message does not contain distortion or projection matrix"); + return; + } - switch(rectifier_impl_) { - case Rectifier::Implementation::NPP: + switch (rectifier_impl_) { + case Rectifier::Implementation::NPP: #if NPP_AVAILABLE - RCLCPP_INFO(this->get_logger(), "Initializing NPP rectifier"); - npp_rectifier_ = std::make_shared(*msg, mapping_impl_, alpha_); - if (npp_rectifier_) { - RCLCPP_INFO(this->get_logger(), "Initialized NPP rectifier"); - rectifier_active_ = true; + RCLCPP_INFO(this->get_logger(), "Initializing NPP rectifier"); + npp_rectifier_ = std::make_shared(*msg, mapping_impl_, alpha_); + if (npp_rectifier_) { + RCLCPP_INFO(this->get_logger(), "Initialized NPP rectifier"); + rectifier_active_ = true; #if JETSON_AVAILABLE || NVJPEG_AVAILABLE - // Use the same stream for rectifier - // because compression process depends on rectified result - auto stream = npp_rectifier_->GetCudaStream(); - rect_compressor_->setCudaStream(stream); + // Use the same stream for rectifier + // because compression process depends on rectified result + auto stream = npp_rectifier_->GetCudaStream(); + rect_compressor_->setCudaStream(stream); #endif - } else { - RCLCPP_ERROR(this->get_logger(), "Failed to initialize NPP rectifier"); - return; - } - break; + } else { + RCLCPP_ERROR(this->get_logger(), "Failed to initialize NPP rectifier"); + return; + } + break; #else - RCLCPP_ERROR(this->get_logger(), "NPP not enabled"); - return; + RCLCPP_ERROR(this->get_logger(), "NPP not enabled"); + return; #endif - case Rectifier::Implementation::OpenCV_CPU: + case Rectifier::Implementation::OpenCV_CPU: #ifdef OPENCV_AVAILABLE - RCLCPP_INFO(this->get_logger(), "Initializing OpenCV CPU rectifier"); - cv_cpu_rectifier_ = std::make_shared(*msg, mapping_impl_, alpha_); - if (cv_cpu_rectifier_) { - RCLCPP_INFO(this->get_logger(), "Initialized OpenCV CPU rectifier"); - rectifier_active_ = true; - } else { - RCLCPP_ERROR(this->get_logger(), "Failed to initialize OpenCV rectifier"); - return; - } - break; + RCLCPP_INFO(this->get_logger(), "Initializing OpenCV CPU rectifier"); + cv_cpu_rectifier_ = + std::make_shared(*msg, mapping_impl_, alpha_); + if (cv_cpu_rectifier_) { + RCLCPP_INFO(this->get_logger(), "Initialized OpenCV CPU rectifier"); + rectifier_active_ = true; + } else { + RCLCPP_ERROR(this->get_logger(), "Failed to initialize OpenCV rectifier"); + return; + } + break; #else - RCLCPP_ERROR(this->get_logger(), "OpenCV not enabled"); - return; + RCLCPP_ERROR(this->get_logger(), "OpenCV not enabled"); + return; #endif - case Rectifier::Implementation::OpenCV_GPU: + case Rectifier::Implementation::OpenCV_GPU: #ifdef OPENCV_CUDA_AVAILABLE - RCLCPP_INFO(this->get_logger(), "Initializing OpenCV GPU rectifier"); - cv_gpu_rectifier_ = std::make_shared(*msg, mapping_impl_, alpha_); - if (cv_gpu_rectifier_) { - RCLCPP_INFO(this->get_logger(), "Initialized OpenCV GPU rectifier"); - rectifier_active_ = true; - } else { - RCLCPP_ERROR(this->get_logger(), "Failed to initialize OpenCV rectifier"); - return; - } - break; + RCLCPP_INFO(this->get_logger(), "Initializing OpenCV GPU rectifier"); + cv_gpu_rectifier_ = + std::make_shared(*msg, mapping_impl_, alpha_); + if (cv_gpu_rectifier_) { + RCLCPP_INFO(this->get_logger(), "Initialized OpenCV GPU rectifier"); + rectifier_active_ = true; + } else { + RCLCPP_ERROR(this->get_logger(), "Failed to initialize OpenCV rectifier"); + return; + } + break; #else - RCLCPP_ERROR(this->get_logger(), "OpenCV CUDA not enabled"); - return; + RCLCPP_ERROR(this->get_logger(), "OpenCV CUDA not enabled"); + return; #endif - default: - RCLCPP_ERROR(this->get_logger(), "Invalid rectifier implementation"); - return; - } - - if (rectifier_active_) { - // unsubscribe - info_sub_.reset(); - } + default: + RCLCPP_ERROR(this->get_logger(), "Invalid rectifier implementation"); + return; + } + + if (rectifier_active_) { + // unsubscribe + info_sub_.reset(); + } } -} // namespace gpu_imgproc +} // namespace gpu_imgproc #include From 08a23f8f943225e20e21320d9179fee44cf11fb6 Mon Sep 17 00:00:00 2001 From: Yuxuan Liu <619684051@qq.com> Date: Fri, 30 May 2025 10:04:58 +0900 Subject: [PATCH 2/3] fix rectify Signed-off-by: Yuxuan Liu <619684051@qq.com> --- include/accelerator/jpeg_compressor.hpp | 1 + include/accelerator/rectifier.hpp | 2 +- src/accelerator/jpeg_compressor.cpp | 30 ++++++++++++++++++------- src/accelerator/rectifier.cpp | 4 ++-- src/gpu_imgproc.cpp | 5 ++--- 5 files changed, 28 insertions(+), 14 deletions(-) diff --git a/include/accelerator/jpeg_compressor.hpp b/include/accelerator/jpeg_compressor.hpp index c4b0b48..621f5d0 100644 --- a/include/accelerator/jpeg_compressor.hpp +++ b/include/accelerator/jpeg_compressor.hpp @@ -84,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: diff --git a/include/accelerator/rectifier.hpp b/include/accelerator/rectifier.hpp index f4194dc..e9fbbbd 100644 --- a/include/accelerator/rectifier.hpp +++ b/include/accelerator/rectifier.hpp @@ -46,7 +46,7 @@ class NPPRectifier { ~NPPRectifier(); cudaStream_t& GetCudaStream() {return stream_;} - std::shared_ptr rectify(const CudaImage &msg); + std::unique_ptr rectify(const CudaImage &msg); private: Npp32f *pxl_map_x_; Npp32f *pxl_map_y_; diff --git a/src/accelerator/jpeg_compressor.cpp b/src/accelerator/jpeg_compressor.cpp index da9b744..9f202a7 100644 --- a/src/accelerator/jpeg_compressor.cpp +++ b/src/accelerator/jpeg_compressor.cpp @@ -94,16 +94,16 @@ JetsonCompressor::~JetsonCompressor() { } } -CompressedImage::UniquePtr compress(const CudaImage &msg, int quality = 90, ImageFormat format = ImageFormat::RGB) { +CompressedImage::UniquePtr JetsonCompressor::compress(const CudaImage &msg, int quality = 90, ImageFormat format = ImageFormat::RGB) { auto ros_image = std::make_unique(); - ros_image->encoding = cuda_msg->encoding; - ros_image->height = cuda_msg->height; - ros_image->width = cuda_msg->width; - ros_image->step = cuda_msg->step; - ros_image->is_bigendian = cuda_msg->is_bigendian; - ros_image->data.resize(cuda_msg->height * ros_image->step); + 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(), cuda_msg->data.get(), ros_image->height * ros_image->step, + ros_image->data.data(), msg.data.get(), ros_image->height * ros_image->step, cudaMemcpyDeviceToHost); return compress(*ros_image, quality, format); @@ -232,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(); + 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(); compressed_msg->header = msg.header; diff --git a/src/accelerator/rectifier.cpp b/src/accelerator/rectifier.cpp index 8139fca..196a4b1 100644 --- a/src/accelerator/rectifier.cpp +++ b/src/accelerator/rectifier.cpp @@ -213,9 +213,9 @@ NPPRectifier::~NPPRectifier() { cudaStreamDestroy(stream_); } -std::shared_ptr NPPRectifier::rectify(const CudaImage &msg) { +std::unique_ptr NPPRectifier::rectify(const CudaImage &msg) { nppSetStream(stream_); - std::shared_ptr result = std::make_shared(); + std::unique_ptr result = std::make_unique(); result->header = msg.header; result->height = msg.height; result->width = msg.width; diff --git a/src/gpu_imgproc.cpp b/src/gpu_imgproc.cpp index 5747d4d..9a55bff 100644 --- a/src/gpu_imgproc.cpp +++ b/src/gpu_imgproc.cpp @@ -202,7 +202,7 @@ void GpuImgProc::imageCallback(std::shared_ptr msg) RCLCPP_DEBUG(this->get_logger(), "Rectifying image"); rectify_task_queue_->addTask([this, msg, image_format]() { // sensor_msgs::msg::Image::UniquePtr rect_img; - std::shared_ptr rect_img; + std::unique_ptr rect_img; sensor_msgs::msg::CompressedImage::UniquePtr rect_comp_img; if (false) { #ifdef NPP_AVAILABLE @@ -221,8 +221,7 @@ void GpuImgProc::imageCallback(std::shared_ptr msg) // rectified_pub_->publish(std::move(rect_img)); // rect_compressed_pub_->publish(std::move(rect_comp_img)); - std::unique_ptr unique_rect_img = std::make_unique(*rect_img); - rectified_pub_->publish(std::move(unique_rect_img)); + rectified_pub_->publish(std::move(rect_img)); rect_compressed_pub_->publish(*rect_comp_img); }); } else { From 100fe13dfc60cea831129b7c58e009b3708e87ae Mon Sep 17 00:00:00 2001 From: bench_perception_ecu2 <619684051@qq.com> Date: Tue, 3 Jun 2025 11:16:06 +0900 Subject: [PATCH 3/3] fix Signed-off-by: bench_perception_ecu2 <619684051@qq.com> --- src/accelerator/jpeg_compressor.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/accelerator/jpeg_compressor.cpp b/src/accelerator/jpeg_compressor.cpp index 9f202a7..4331f02 100644 --- a/src/accelerator/jpeg_compressor.cpp +++ b/src/accelerator/jpeg_compressor.cpp @@ -94,7 +94,7 @@ JetsonCompressor::~JetsonCompressor() { } } -CompressedImage::UniquePtr JetsonCompressor::compress(const CudaImage &msg, int quality = 90, ImageFormat format = ImageFormat::RGB) { +CompressedImage::UniquePtr JetsonCompressor::compress(const CudaImage &msg, int quality, ImageFormat format) { auto ros_image = std::make_unique(); ros_image->encoding = msg.encoding; ros_image->height = msg.height;