diff --git a/image_proc/include/image_proc/resize.hpp b/image_proc/include/image_proc/resize.hpp index 6901ad1fe..d51bca4b9 100644 --- a/image_proc/include/image_proc/resize.hpp +++ b/image_proc/include/image_proc/resize.hpp @@ -60,6 +60,8 @@ class ResizeNode int height_; int width_; + cv_bridge::CvImage scaled_cv_; + std::mutex connect_mutex_; void connectCb(); diff --git a/image_proc/src/resize.cpp b/image_proc/src/resize.cpp index 8795360d6..628f09e64 100644 --- a/image_proc/src/resize.cpp +++ b/image_proc/src/resize.cpp @@ -90,10 +90,10 @@ void ResizeNode::imageCb( static_cast(&(*image_msg)), static_cast(&(*info_msg))); - cv_bridge::CvImagePtr cv_ptr; + cv_bridge::CvImageConstPtr cv_ptr; try { - cv_ptr = cv_bridge::toCvCopy(image_msg); + cv_ptr = cv_bridge::toCvShare(image_msg); } catch (cv_bridge::Exception & e) { TRACEPOINT( image_proc_resize_fini, @@ -106,12 +106,12 @@ void ResizeNode::imageCb( if (use_scale_) { cv::resize( - cv_ptr->image, cv_ptr->image, cv::Size(0, 0), scale_width_, + cv_ptr->image, scaled_cv_.image, cv::Size(0, 0), scale_width_, scale_height_, interpolation_); } else { int height = height_ == -1 ? image_msg->height : height_; int width = width_ == -1 ? image_msg->width : width_; - cv::resize(cv_ptr->image, cv_ptr->image, cv::Size(width, height), 0, 0, interpolation_); + cv::resize(cv_ptr->image, scaled_cv_.image, cv::Size(width, height), 0, 0, interpolation_); } sensor_msgs::msg::CameraInfo::SharedPtr dst_info_msg = @@ -148,7 +148,9 @@ void ResizeNode::imageCb( dst_info_msg->roi.width = static_cast(dst_info_msg->roi.width * scale_x); dst_info_msg->roi.height = static_cast(dst_info_msg->roi.height * scale_y); - pub_image_.publish(*cv_ptr->toImageMsg(), *dst_info_msg); + scaled_cv_.header = image_msg->header; + scaled_cv_.encoding = image_msg->encoding; + pub_image_.publish(*scaled_cv_.toImageMsg(), *dst_info_msg); TRACEPOINT( image_proc_resize_fini,