From e5e6026ad969875779db5b44f512f366d2452733 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?B=C5=82a=C5=BCej=20Sowa?= Date: Tue, 20 Aug 2024 10:41:01 +0200 Subject: [PATCH] Publish using unique ptr (#1016) Prevents doing an extra copy of the data when using intra-process communication. --- .../include/depth_image_proc/conversions.hpp | 34 +++++++++---------- depth_image_proc/src/conversions.cpp | 14 ++++---- depth_image_proc/src/convert_metric.cpp | 4 +-- depth_image_proc/src/crop_foremost.cpp | 4 ++- depth_image_proc/src/disparity.cpp | 16 ++++----- depth_image_proc/src/point_cloud_xyz.cpp | 8 ++--- .../src/point_cloud_xyz_radial.cpp | 8 ++--- depth_image_proc/src/point_cloud_xyzi.cpp | 16 ++++----- .../src/point_cloud_xyzi_radial.cpp | 16 ++++----- depth_image_proc/src/point_cloud_xyzrgb.cpp | 18 +++++----- .../src/point_cloud_xyzrgb_radial.cpp | 18 +++++----- depth_image_proc/src/register.cpp | 34 +++++++++---------- image_proc/src/crop_decimate.cpp | 8 ++--- image_proc/src/crop_non_zero.cpp | 9 +++-- image_proc/src/debayer.cpp | 29 +++++++++------- image_proc/src/rectify.cpp | 6 ++-- image_proc/src/resize.cpp | 9 +++-- image_publisher/src/image_publisher.cpp | 12 ++++--- image_rotate/src/image_rotate_node.cpp | 6 ++-- .../src/stereo_image_proc/disparity_node.cpp | 4 +-- .../stereo_image_proc/point_cloud_node.cpp | 4 +-- 21 files changed, 145 insertions(+), 132 deletions(-) diff --git a/depth_image_proc/include/depth_image_proc/conversions.hpp b/depth_image_proc/include/depth_image_proc/conversions.hpp index a8f0e53c6..4221eaa79 100644 --- a/depth_image_proc/include/depth_image_proc/conversions.hpp +++ b/depth_image_proc/include/depth_image_proc/conversions.hpp @@ -51,7 +51,7 @@ namespace depth_image_proc template void convertDepth( const sensor_msgs::msg::Image::ConstSharedPtr & depth_msg, - const sensor_msgs::msg::PointCloud2::SharedPtr & cloud_msg, + sensor_msgs::msg::PointCloud2 & cloud_msg, const image_geometry::PinholeCameraModel & model, double invalid_depth = 0.0) { @@ -71,14 +71,14 @@ void convertDepth( if (use_invalid_depth) { invalid_depth_cvt = DepthTraits::fromMeters(invalid_depth); } - sensor_msgs::PointCloud2Iterator iter_x(*cloud_msg, "x"); - sensor_msgs::PointCloud2Iterator iter_y(*cloud_msg, "y"); - sensor_msgs::PointCloud2Iterator iter_z(*cloud_msg, "z"); + sensor_msgs::PointCloud2Iterator iter_x(cloud_msg, "x"); + sensor_msgs::PointCloud2Iterator iter_y(cloud_msg, "y"); + sensor_msgs::PointCloud2Iterator iter_z(cloud_msg, "z"); const T * depth_row = reinterpret_cast(&depth_msg->data[0]); uint32_t row_step = depth_msg->step / sizeof(T); - for (uint32_t v = 0; v < cloud_msg->height; ++v, depth_row += row_step) { - for (uint32_t u = 0; u < cloud_msg->width; ++u, ++iter_x, ++iter_y, ++iter_z) { + for (uint32_t v = 0; v < cloud_msg.height; ++v, depth_row += row_step) { + for (uint32_t u = 0; u < cloud_msg.width; ++u, ++iter_x, ++iter_y, ++iter_z) { T depth = depth_row[u]; // Missing points denoted by NaNs @@ -103,19 +103,19 @@ void convertDepth( template void convertDepthRadial( const sensor_msgs::msg::Image::ConstSharedPtr & depth_msg, - const sensor_msgs::msg::PointCloud2::SharedPtr & cloud_msg, + sensor_msgs::msg::PointCloud2 & cloud_msg, const cv::Mat & transform) { // Combine unit conversion (if necessary) with scaling by focal length for computing (X,Y) float bad_point = std::numeric_limits::quiet_NaN(); - sensor_msgs::PointCloud2Iterator iter_x(*cloud_msg, "x"); - sensor_msgs::PointCloud2Iterator iter_y(*cloud_msg, "y"); - sensor_msgs::PointCloud2Iterator iter_z(*cloud_msg, "z"); + sensor_msgs::PointCloud2Iterator iter_x(cloud_msg, "x"); + sensor_msgs::PointCloud2Iterator iter_y(cloud_msg, "y"); + sensor_msgs::PointCloud2Iterator iter_z(cloud_msg, "z"); const T * depth_row = reinterpret_cast(&depth_msg->data[0]); int row_step = depth_msg->step / sizeof(T); - for (int v = 0; v < static_cast(cloud_msg->height); ++v, depth_row += row_step) { - for (int u = 0; u < static_cast(cloud_msg->width); ++u, ++iter_x, ++iter_y, ++iter_z) { + for (int v = 0; v < static_cast(cloud_msg.height); ++v, depth_row += row_step) { + for (int u = 0; u < static_cast(cloud_msg.width); ++u, ++iter_x, ++iter_y, ++iter_z) { T depth = depth_row[u]; // Missing points denoted by NaNs @@ -136,14 +136,14 @@ void convertDepthRadial( template void convertIntensity( const sensor_msgs::msg::Image::ConstSharedPtr & intensity_msg, - const sensor_msgs::msg::PointCloud2::SharedPtr & cloud_msg) + sensor_msgs::msg::PointCloud2 & cloud_msg) { - sensor_msgs::PointCloud2Iterator iter_i(*cloud_msg, "intensity"); + sensor_msgs::PointCloud2Iterator iter_i(cloud_msg, "intensity"); const T * inten_row = reinterpret_cast(&intensity_msg->data[0]); const int i_row_step = intensity_msg->step / sizeof(T); - for (int v = 0; v < static_cast(cloud_msg->height); ++v, inten_row += i_row_step) { - for (int u = 0; u < static_cast(cloud_msg->width); ++u, ++iter_i) { + for (int v = 0; v < static_cast(cloud_msg.height); ++v, inten_row += i_row_step) { + for (int u = 0; u < static_cast(cloud_msg.width); ++u, ++iter_i) { *iter_i = inten_row[u]; } } @@ -152,7 +152,7 @@ void convertIntensity( // Handles RGB8, BGR8, and MONO8 void convertRgb( const sensor_msgs::msg::Image::ConstSharedPtr & rgb_msg, - const sensor_msgs::msg::PointCloud2::SharedPtr & cloud_msg, + sensor_msgs::msg::PointCloud2 & cloud_msg, int red_offset, int green_offset, int blue_offset, int color_step); cv::Mat initMatrix(cv::Mat cameraMatrix, cv::Mat distCoeffs, int width, int height, bool radial); diff --git a/depth_image_proc/src/conversions.cpp b/depth_image_proc/src/conversions.cpp index 697734414..e5c1b2052 100644 --- a/depth_image_proc/src/conversions.cpp +++ b/depth_image_proc/src/conversions.cpp @@ -79,16 +79,16 @@ cv::Mat initMatrix( void convertRgb( const sensor_msgs::msg::Image::ConstSharedPtr & rgb_msg, - const sensor_msgs::msg::PointCloud2::SharedPtr & cloud_msg, + sensor_msgs::msg::PointCloud2 & cloud_msg, int red_offset, int green_offset, int blue_offset, int color_step) { - sensor_msgs::PointCloud2Iterator iter_r(*cloud_msg, "r"); - sensor_msgs::PointCloud2Iterator iter_g(*cloud_msg, "g"); - sensor_msgs::PointCloud2Iterator iter_b(*cloud_msg, "b"); + sensor_msgs::PointCloud2Iterator iter_r(cloud_msg, "r"); + sensor_msgs::PointCloud2Iterator iter_g(cloud_msg, "g"); + sensor_msgs::PointCloud2Iterator iter_b(cloud_msg, "b"); const uint8_t * rgb = &rgb_msg->data[0]; int rgb_skip = rgb_msg->step - rgb_msg->width * color_step; - for (int v = 0; v < static_cast(cloud_msg->height); ++v, rgb += rgb_skip) { - for (int u = 0; u < static_cast(cloud_msg->width); ++u, + for (int v = 0; v < static_cast(cloud_msg.height); ++v, rgb += rgb_skip) { + for (int u = 0; u < static_cast(cloud_msg.width); ++u, rgb += color_step, ++iter_r, ++iter_g, ++iter_b) { *iter_r = rgb[red_offset]; @@ -101,7 +101,7 @@ void convertRgb( // force template instantiation template void convertDepth( const sensor_msgs::msg::Image::ConstSharedPtr & depth_msg, - const sensor_msgs::msg::PointCloud2::SharedPtr & cloud_msg, + sensor_msgs::msg::PointCloud2 & cloud_msg, const image_geometry::PinholeCameraModel & model, double invalid_depth); diff --git a/depth_image_proc/src/convert_metric.cpp b/depth_image_proc/src/convert_metric.cpp index 849e197d0..7ef841366 100644 --- a/depth_image_proc/src/convert_metric.cpp +++ b/depth_image_proc/src/convert_metric.cpp @@ -106,7 +106,7 @@ ConvertMetricNode::ConvertMetricNode(const rclcpp::NodeOptions & options) void ConvertMetricNode::depthCb(const sensor_msgs::msg::Image::ConstSharedPtr & raw_msg) { - auto depth_msg = std::make_shared(); + auto depth_msg = std::make_unique(); depth_msg->header = raw_msg->header; depth_msg->height = raw_msg->height; depth_msg->width = raw_msg->width; @@ -143,7 +143,7 @@ void ConvertMetricNode::depthCb(const sensor_msgs::msg::Image::ConstSharedPtr & return; } - pub_depth_.publish(depth_msg); + pub_depth_.publish(std::move(depth_msg)); } } // namespace depth_image_proc diff --git a/depth_image_proc/src/crop_foremost.cpp b/depth_image_proc/src/crop_foremost.cpp index ddfd845ee..965018c23 100644 --- a/depth_image_proc/src/crop_foremost.cpp +++ b/depth_image_proc/src/crop_foremost.cpp @@ -152,7 +152,9 @@ void CropForemostNode::depthCb(const sensor_msgs::msg::Image::ConstSharedPtr & r break; } - pub_depth_.publish(cv_ptr->toImageMsg()); + auto image_msg = std::make_unique(); + cv_ptr->toImageMsg(*image_msg); + pub_depth_.publish(std::move(image_msg)); } } // namespace depth_image_proc diff --git a/depth_image_proc/src/disparity.cpp b/depth_image_proc/src/disparity.cpp index ef6458471..86db0c73a 100644 --- a/depth_image_proc/src/disparity.cpp +++ b/depth_image_proc/src/disparity.cpp @@ -77,7 +77,7 @@ class DisparityNode : public rclcpp::Node template void convert( const sensor_msgs::msg::Image::ConstSharedPtr & depth_msg, - stereo_msgs::msg::DisparityImage::SharedPtr & disp_msg); + stereo_msgs::msg::DisparityImage & disp_msg); }; DisparityNode::DisparityNode(const rclcpp::NodeOptions & options) @@ -129,7 +129,7 @@ void DisparityNode::depthCb( const sensor_msgs::msg::Image::ConstSharedPtr & depth_msg, const sensor_msgs::msg::CameraInfo::ConstSharedPtr & info_msg) { - auto disp_msg = std::make_shared(); + auto disp_msg = std::make_unique(); disp_msg->header = depth_msg->header; disp_msg->image.header = disp_msg->header; disp_msg->image.encoding = sensor_msgs::image_encodings::TYPE_32FC1; @@ -146,30 +146,30 @@ void DisparityNode::depthCb( disp_msg->delta_d = delta_d_; if (depth_msg->encoding == sensor_msgs::image_encodings::TYPE_16UC1) { - convert(depth_msg, disp_msg); + convert(depth_msg, *disp_msg); } else if (depth_msg->encoding == sensor_msgs::image_encodings::TYPE_32FC1) { - convert(depth_msg, disp_msg); + convert(depth_msg, *disp_msg); } else { RCLCPP_ERROR( get_logger(), "Depth image has unsupported encoding [%s]", depth_msg->encoding.c_str()); return; } - pub_disparity_->publish(*disp_msg); + pub_disparity_->publish(std::move(disp_msg)); } template void DisparityNode::convert( const sensor_msgs::msg::Image::ConstSharedPtr & depth_msg, - stereo_msgs::msg::DisparityImage::SharedPtr & disp_msg) + stereo_msgs::msg::DisparityImage & disp_msg) { // For each depth Z, disparity d = fT / Z float unit_scaling = DepthTraits::toMeters(T(1) ); - float constant = disp_msg->f * disp_msg->t / unit_scaling; + float constant = disp_msg.f * disp_msg.t / unit_scaling; const T * depth_row = reinterpret_cast(&depth_msg->data[0]); int row_step = depth_msg->step / sizeof(T); - float * disp_data = reinterpret_cast(&disp_msg->image.data[0]); + float * disp_data = reinterpret_cast(&disp_msg.image.data[0]); for (int v = 0; v < static_cast(depth_msg->height); ++v) { for (int u = 0; u < static_cast(depth_msg->width); ++u) { T depth = depth_row[u]; diff --git a/depth_image_proc/src/point_cloud_xyz.cpp b/depth_image_proc/src/point_cloud_xyz.cpp index d85a56748..6f6074ad0 100644 --- a/depth_image_proc/src/point_cloud_xyz.cpp +++ b/depth_image_proc/src/point_cloud_xyz.cpp @@ -104,7 +104,7 @@ void PointCloudXyzNode::depthCb( const Image::ConstSharedPtr & depth_msg, const CameraInfo::ConstSharedPtr & info_msg) { - const PointCloud2::SharedPtr cloud_msg = std::make_shared(); + auto cloud_msg = std::make_unique(); cloud_msg->header = depth_msg->header; cloud_msg->height = depth_msg->height; cloud_msg->width = depth_msg->width; @@ -119,16 +119,16 @@ void PointCloudXyzNode::depthCb( // Convert Depth Image to Pointcloud if (depth_msg->encoding == enc::TYPE_16UC1 || depth_msg->encoding == enc::MONO16) { - convertDepth(depth_msg, cloud_msg, model_, invalid_depth_); + convertDepth(depth_msg, *cloud_msg, model_, invalid_depth_); } else if (depth_msg->encoding == enc::TYPE_32FC1) { - convertDepth(depth_msg, cloud_msg, model_, invalid_depth_); + convertDepth(depth_msg, *cloud_msg, model_, invalid_depth_); } else { RCLCPP_ERROR( get_logger(), "Depth image has unsupported encoding [%s]", depth_msg->encoding.c_str()); return; } - pub_point_cloud_->publish(*cloud_msg); + pub_point_cloud_->publish(std::move(cloud_msg)); } } // namespace depth_image_proc diff --git a/depth_image_proc/src/point_cloud_xyz_radial.cpp b/depth_image_proc/src/point_cloud_xyz_radial.cpp index 4407de7c5..a7f48be30 100644 --- a/depth_image_proc/src/point_cloud_xyz_radial.cpp +++ b/depth_image_proc/src/point_cloud_xyz_radial.cpp @@ -97,7 +97,7 @@ void PointCloudXyzRadialNode::depthCb( const sensor_msgs::msg::Image::ConstSharedPtr & depth_msg, const sensor_msgs::msg::CameraInfo::ConstSharedPtr & info_msg) { - auto cloud_msg = std::make_shared(); + auto cloud_msg = std::make_unique(); cloud_msg->header = depth_msg->header; cloud_msg->height = depth_msg->height; cloud_msg->width = depth_msg->width; @@ -119,16 +119,16 @@ void PointCloudXyzRadialNode::depthCb( // Convert Depth Image to Pointcloud if (depth_msg->encoding == sensor_msgs::image_encodings::TYPE_16UC1) { - convertDepthRadial(depth_msg, cloud_msg, transform_); + convertDepthRadial(depth_msg, *cloud_msg, transform_); } else if (depth_msg->encoding == sensor_msgs::image_encodings::TYPE_32FC1) { - convertDepthRadial(depth_msg, cloud_msg, transform_); + convertDepthRadial(depth_msg, *cloud_msg, transform_); } else { RCLCPP_ERROR( get_logger(), "Depth image has unsupported encoding [%s]", depth_msg->encoding.c_str()); return; } - pub_point_cloud_->publish(*cloud_msg); + pub_point_cloud_->publish(std::move(cloud_msg)); } } // namespace depth_image_proc diff --git a/depth_image_proc/src/point_cloud_xyzi.cpp b/depth_image_proc/src/point_cloud_xyzi.cpp index 6adf15922..2ff520479 100644 --- a/depth_image_proc/src/point_cloud_xyzi.cpp +++ b/depth_image_proc/src/point_cloud_xyzi.cpp @@ -190,7 +190,7 @@ void PointCloudXyziNode::imageCb( } } - auto cloud_msg = std::make_shared(); + auto cloud_msg = std::make_unique(); cloud_msg->header = depth_msg->header; // Use depth image time stamp cloud_msg->height = depth_msg->height; cloud_msg->width = depth_msg->width; @@ -208,9 +208,9 @@ void PointCloudXyziNode::imageCb( // Convert Depth Image to Pointcloud if (depth_msg->encoding == enc::TYPE_16UC1) { - convertDepth(depth_msg, cloud_msg, model_, invalid_depth_); + convertDepth(depth_msg, *cloud_msg, model_, invalid_depth_); } else if (depth_msg->encoding == enc::TYPE_32FC1) { - convertDepth(depth_msg, cloud_msg, model_, invalid_depth_); + convertDepth(depth_msg, *cloud_msg, model_, invalid_depth_); } else { RCLCPP_ERROR( get_logger(), "Depth image has unsupported encoding [%s]", depth_msg->encoding.c_str()); @@ -219,13 +219,13 @@ void PointCloudXyziNode::imageCb( // Convert Intensity Image to Pointcloud if (intensity_msg->encoding == enc::MONO8) { - convertIntensity(intensity_msg, cloud_msg); + convertIntensity(intensity_msg, *cloud_msg); } else if (intensity_msg->encoding == enc::MONO16) { - convertIntensity(intensity_msg, cloud_msg); + convertIntensity(intensity_msg, *cloud_msg); } else if (intensity_msg->encoding == enc::TYPE_16UC1) { - convertIntensity(intensity_msg, cloud_msg); + convertIntensity(intensity_msg, *cloud_msg); } else if (intensity_msg->encoding == enc::TYPE_32FC1) { - convertIntensity(intensity_msg, cloud_msg); + convertIntensity(intensity_msg, *cloud_msg); } else { RCLCPP_ERROR( get_logger(), "Intensity image has unsupported encoding [%s]", @@ -233,7 +233,7 @@ void PointCloudXyziNode::imageCb( return; } - pub_point_cloud_->publish(*cloud_msg); + pub_point_cloud_->publish(std::move(cloud_msg)); } diff --git a/depth_image_proc/src/point_cloud_xyzi_radial.cpp b/depth_image_proc/src/point_cloud_xyzi_radial.cpp index 2e7132cd8..e89ee5a05 100644 --- a/depth_image_proc/src/point_cloud_xyzi_radial.cpp +++ b/depth_image_proc/src/point_cloud_xyzi_radial.cpp @@ -123,7 +123,7 @@ void PointCloudXyziRadialNode::imageCb( const Image::ConstSharedPtr & intensity_msg, const CameraInfo::ConstSharedPtr & info_msg) { - auto cloud_msg = std::make_shared(); + auto cloud_msg = std::make_unique(); cloud_msg->header = depth_msg->header; cloud_msg->height = depth_msg->height; cloud_msg->width = depth_msg->width; @@ -151,9 +151,9 @@ void PointCloudXyziRadialNode::imageCb( // Convert Depth Image to Pointcloud if (depth_msg->encoding == sensor_msgs::image_encodings::TYPE_16UC1) { - convertDepthRadial(depth_msg, cloud_msg, transform_); + convertDepthRadial(depth_msg, *cloud_msg, transform_); } else if (depth_msg->encoding == sensor_msgs::image_encodings::TYPE_32FC1) { - convertDepthRadial(depth_msg, cloud_msg, transform_); + convertDepthRadial(depth_msg, *cloud_msg, transform_); } else { RCLCPP_ERROR( get_logger(), "Depth image has unsupported encoding [%s]", depth_msg->encoding.c_str()); @@ -161,13 +161,13 @@ void PointCloudXyziRadialNode::imageCb( } if (intensity_msg->encoding == sensor_msgs::image_encodings::MONO8) { - convertIntensity(intensity_msg, cloud_msg); + convertIntensity(intensity_msg, *cloud_msg); } else if (intensity_msg->encoding == sensor_msgs::image_encodings::MONO16) { - convertIntensity(intensity_msg, cloud_msg); + convertIntensity(intensity_msg, *cloud_msg); } else if (intensity_msg->encoding == sensor_msgs::image_encodings::TYPE_16UC1) { - convertIntensity(intensity_msg, cloud_msg); + convertIntensity(intensity_msg, *cloud_msg); } else if (intensity_msg->encoding == sensor_msgs::image_encodings::TYPE_32FC1) { - convertIntensity(intensity_msg, cloud_msg); + convertIntensity(intensity_msg, *cloud_msg); } else { RCLCPP_ERROR( get_logger(), "Intensity image has unsupported encoding [%s]", @@ -175,7 +175,7 @@ void PointCloudXyziRadialNode::imageCb( return; } - pub_point_cloud_->publish(*cloud_msg); + pub_point_cloud_->publish(std::move(cloud_msg)); } } // namespace depth_image_proc diff --git a/depth_image_proc/src/point_cloud_xyzrgb.cpp b/depth_image_proc/src/point_cloud_xyzrgb.cpp index 5bd904d11..0f502be96 100644 --- a/depth_image_proc/src/point_cloud_xyzrgb.cpp +++ b/depth_image_proc/src/point_cloud_xyzrgb.cpp @@ -251,7 +251,7 @@ void PointCloudXyzrgbNode::imageCb( color_step = 3; } - auto cloud_msg = std::make_shared(); + auto cloud_msg = std::make_unique(); cloud_msg->header = depth_msg->header; // Use depth image time stamp cloud_msg->height = depth_msg->height; cloud_msg->width = depth_msg->width; @@ -263,9 +263,9 @@ void PointCloudXyzrgbNode::imageCb( // Convert Depth Image to Pointcloud if (depth_msg->encoding == sensor_msgs::image_encodings::TYPE_16UC1) { - convertDepth(depth_msg, cloud_msg, model_, invalid_depth_); + convertDepth(depth_msg, *cloud_msg, model_, invalid_depth_); } else if (depth_msg->encoding == sensor_msgs::image_encodings::TYPE_32FC1) { - convertDepth(depth_msg, cloud_msg, model_, invalid_depth_); + convertDepth(depth_msg, *cloud_msg, model_, invalid_depth_); } else { RCLCPP_ERROR( get_logger(), "Depth image has unsupported encoding [%s]", depth_msg->encoding.c_str()); @@ -274,22 +274,22 @@ void PointCloudXyzrgbNode::imageCb( // Convert RGB if (rgb_msg->encoding == sensor_msgs::image_encodings::RGB8) { - convertRgb(rgb_msg, cloud_msg, red_offset, green_offset, blue_offset, color_step); + convertRgb(rgb_msg, *cloud_msg, red_offset, green_offset, blue_offset, color_step); } else if (rgb_msg->encoding == sensor_msgs::image_encodings::BGR8) { - convertRgb(rgb_msg, cloud_msg, red_offset, green_offset, blue_offset, color_step); + convertRgb(rgb_msg, *cloud_msg, red_offset, green_offset, blue_offset, color_step); } else if (rgb_msg->encoding == sensor_msgs::image_encodings::BGRA8) { - convertRgb(rgb_msg, cloud_msg, red_offset, green_offset, blue_offset, color_step); + convertRgb(rgb_msg, *cloud_msg, red_offset, green_offset, blue_offset, color_step); } else if (rgb_msg->encoding == sensor_msgs::image_encodings::RGBA8) { - convertRgb(rgb_msg, cloud_msg, red_offset, green_offset, blue_offset, color_step); + convertRgb(rgb_msg, *cloud_msg, red_offset, green_offset, blue_offset, color_step); } else if (rgb_msg->encoding == sensor_msgs::image_encodings::MONO8) { - convertRgb(rgb_msg, cloud_msg, red_offset, green_offset, blue_offset, color_step); + convertRgb(rgb_msg, *cloud_msg, red_offset, green_offset, blue_offset, color_step); } else { RCLCPP_ERROR( get_logger(), "RGB image has unsupported encoding [%s]", rgb_msg->encoding.c_str()); return; } - pub_point_cloud_->publish(*cloud_msg); + pub_point_cloud_->publish(std::move(cloud_msg)); } } // namespace depth_image_proc diff --git a/depth_image_proc/src/point_cloud_xyzrgb_radial.cpp b/depth_image_proc/src/point_cloud_xyzrgb_radial.cpp index 9c0197e78..0694e3f8a 100644 --- a/depth_image_proc/src/point_cloud_xyzrgb_radial.cpp +++ b/depth_image_proc/src/point_cloud_xyzrgb_radial.cpp @@ -242,7 +242,7 @@ void PointCloudXyzrgbRadialNode::imageCb( color_step = 3; } - auto cloud_msg = std::make_shared(); + auto cloud_msg = std::make_unique(); cloud_msg->header = depth_msg->header; // Use depth image time stamp cloud_msg->height = depth_msg->height; cloud_msg->width = depth_msg->width; @@ -254,9 +254,9 @@ void PointCloudXyzrgbRadialNode::imageCb( // Convert Depth Image to Pointcloud if (depth_msg->encoding == sensor_msgs::image_encodings::TYPE_16UC1) { - convertDepthRadial(depth_msg, cloud_msg, transform_); + convertDepthRadial(depth_msg, *cloud_msg, transform_); } else if (depth_msg->encoding == sensor_msgs::image_encodings::TYPE_32FC1) { - convertDepthRadial(depth_msg, cloud_msg, transform_); + convertDepthRadial(depth_msg, *cloud_msg, transform_); } else { RCLCPP_ERROR( get_logger(), "Depth image has unsupported encoding [%s]", depth_msg->encoding.c_str()); @@ -265,22 +265,22 @@ void PointCloudXyzrgbRadialNode::imageCb( // Convert RGB if (rgb_msg->encoding == sensor_msgs::image_encodings::RGB8) { - convertRgb(rgb_msg, cloud_msg, red_offset, green_offset, blue_offset, color_step); + convertRgb(rgb_msg, *cloud_msg, red_offset, green_offset, blue_offset, color_step); } else if (rgb_msg->encoding == sensor_msgs::image_encodings::BGR8) { - convertRgb(rgb_msg, cloud_msg, red_offset, green_offset, blue_offset, color_step); + convertRgb(rgb_msg, *cloud_msg, red_offset, green_offset, blue_offset, color_step); } else if (rgb_msg->encoding == sensor_msgs::image_encodings::BGRA8) { - convertRgb(rgb_msg, cloud_msg, red_offset, green_offset, blue_offset, color_step); + convertRgb(rgb_msg, *cloud_msg, red_offset, green_offset, blue_offset, color_step); } else if (rgb_msg->encoding == sensor_msgs::image_encodings::RGBA8) { - convertRgb(rgb_msg, cloud_msg, red_offset, green_offset, blue_offset, color_step); + convertRgb(rgb_msg, *cloud_msg, red_offset, green_offset, blue_offset, color_step); } else if (rgb_msg->encoding == sensor_msgs::image_encodings::MONO8) { - convertRgb(rgb_msg, cloud_msg, red_offset, green_offset, blue_offset, color_step); + convertRgb(rgb_msg, *cloud_msg, red_offset, green_offset, blue_offset, color_step); } else { RCLCPP_ERROR( get_logger(), "RGB image has unsupported encoding [%s]", rgb_msg->encoding.c_str()); return; } - pub_point_cloud_->publish(*cloud_msg); + pub_point_cloud_->publish(std::move(cloud_msg)); } } // namespace depth_image_proc diff --git a/depth_image_proc/src/register.cpp b/depth_image_proc/src/register.cpp index 06a721d6c..7db288436 100644 --- a/depth_image_proc/src/register.cpp +++ b/depth_image_proc/src/register.cpp @@ -92,7 +92,7 @@ class RegisterNode : public rclcpp::Node template void convert( const Image::ConstSharedPtr & depth_msg, - const Image::SharedPtr & registered_msg, + Image & registered_msg, const Eigen::Affine3d & depth_to_rgb); }; @@ -185,7 +185,7 @@ void RegisterNode::imageCb( /// don't call publish() in this cb. What's going on roscpp? } - auto registered_msg = std::make_shared(); + auto registered_msg = std::make_unique(); registered_msg->header.stamp = use_rgb_timestamp_ ? rgb_info_msg->header.stamp : depth_image_msg->header.stamp; registered_msg->header.frame_id = rgb_info_msg->header.frame_id; @@ -197,9 +197,9 @@ void RegisterNode::imageCb( // step and data set in convert(), depend on depth data type if (depth_image_msg->encoding == sensor_msgs::image_encodings::TYPE_16UC1) { - convert(depth_image_msg, registered_msg, depth_to_rgb); + convert(depth_image_msg, *registered_msg, depth_to_rgb); } else if (depth_image_msg->encoding == sensor_msgs::image_encodings::TYPE_32FC1) { - convert(depth_image_msg, registered_msg, depth_to_rgb); + convert(depth_image_msg, *registered_msg, depth_to_rgb); } else { RCLCPP_ERROR( get_logger(), "Depth image has unsupported encoding [%s]", @@ -208,24 +208,24 @@ void RegisterNode::imageCb( } // Registered camera info is the same as the RGB info, but uses the depth timestamp - auto registered_info_msg = std::make_shared(*rgb_info_msg); + auto registered_info_msg = std::make_unique(*rgb_info_msg); registered_info_msg->header.stamp = registered_msg->header.stamp; - pub_registered_.publish(registered_msg, registered_info_msg); + pub_registered_.publish(std::move(registered_msg), std::move(registered_info_msg)); } template void RegisterNode::convert( const Image::ConstSharedPtr & depth_msg, - const Image::SharedPtr & registered_msg, + Image & registered_msg, const Eigen::Affine3d & depth_to_rgb) { // Allocate memory for registered depth image - registered_msg->step = registered_msg->width * sizeof(T); - registered_msg->data.resize(registered_msg->height * registered_msg->step); + registered_msg.step = registered_msg.width * sizeof(T); + registered_msg.data.resize(registered_msg.height * registered_msg.step); // data is already zero-filled in the uint16 case, // but for floats we want to initialize everything to NaN. - DepthTraits::initializeBuffer(registered_msg->data); + DepthTraits::initializeBuffer(registered_msg.data); // Extract all the parameters we need double inv_depth_fx = 1.0 / depth_model_.fx(); @@ -241,7 +241,7 @@ void RegisterNode::convert( // registered image const T * depth_row = reinterpret_cast(&depth_msg->data[0]); int row_step = depth_msg->step / sizeof(T); - T * registered_data = reinterpret_cast(®istered_msg->data[0]); + T * registered_data = reinterpret_cast(®istered_msg.data[0]); int raw_index = 0; for (unsigned v = 0; v < depth_msg->height; ++v, depth_row += row_step) { for (unsigned u = 0; u < depth_msg->width; ++u, ++raw_index) { @@ -269,13 +269,13 @@ void RegisterNode::convert( int u_rgb = (rgb_fx * xyz_rgb.x() + rgb_Tx) * inv_Z + rgb_cx + 0.5; int v_rgb = (rgb_fy * xyz_rgb.y() + rgb_Ty) * inv_Z + rgb_cy + 0.5; - if (u_rgb < 0 || u_rgb >= static_cast(registered_msg->width) || - v_rgb < 0 || v_rgb >= static_cast(registered_msg->height)) + if (u_rgb < 0 || u_rgb >= static_cast(registered_msg.width) || + v_rgb < 0 || v_rgb >= static_cast(registered_msg.height)) { continue; } - T & reg_depth = registered_data[v_rgb * registered_msg->width + u_rgb]; + T & reg_depth = registered_data[v_rgb * registered_msg.width + u_rgb]; T new_depth = DepthTraits::fromMeters(xyz_rgb.z()); // Validity and Z-buffer checks if (!DepthTraits::valid(reg_depth) || reg_depth > new_depth) { @@ -305,15 +305,15 @@ void RegisterNode::convert( int u_rgb_2 = (rgb_fx * xyz_rgb_2.x() + rgb_Tx) * inv_Z + rgb_cx + 0.5; int v_rgb_2 = (rgb_fy * xyz_rgb_2.y() + rgb_Ty) * inv_Z + rgb_cy + 0.5; - if (u_rgb_1 < 0 || u_rgb_2 >= static_cast(registered_msg->width) || - v_rgb_1 < 0 || v_rgb_2 >= static_cast(registered_msg->height)) + if (u_rgb_1 < 0 || u_rgb_2 >= static_cast(registered_msg.width) || + v_rgb_1 < 0 || v_rgb_2 >= static_cast(registered_msg.height)) { continue; } for (int nv = v_rgb_1; nv <= v_rgb_2; ++nv) { for (int nu = u_rgb_1; nu <= u_rgb_2; ++nu) { - T & reg_depth = registered_data[nv * registered_msg->width + nu]; + T & reg_depth = registered_data[nv * registered_msg.width + nu]; T new_depth = DepthTraits::fromMeters(0.5 * (xyz_rgb_1.z() + xyz_rgb_2.z())); // Validity and Z-buffer checks if (!DepthTraits::valid(reg_depth) || reg_depth > new_depth) { diff --git a/image_proc/src/crop_decimate.cpp b/image_proc/src/crop_decimate.cpp index 91169afbd..29a18d5e0 100644 --- a/image_proc/src/crop_decimate.cpp +++ b/image_proc/src/crop_decimate.cpp @@ -325,11 +325,11 @@ void CropDecimateNode::imageCb( // Create output Image message /// @todo Could save copies by allocating this above and having output.image alias it - sensor_msgs::msg::Image::SharedPtr out_image = output.toImageMsg(); + auto out_image = std::make_unique(); + output.toImageMsg(*out_image); // Create updated CameraInfo message - sensor_msgs::msg::CameraInfo::SharedPtr out_info = - std::make_shared(*info_msg); + auto out_info = std::make_unique(*info_msg); int binning_x = std::max(static_cast(info_msg->binning_x), 1); int binning_y = std::max(static_cast(info_msg->binning_y), 1); out_info->binning_x = binning_x * decimation_x_; @@ -351,7 +351,7 @@ void CropDecimateNode::imageCb( out_info->header.frame_id = target_frame_id_; } - pub_.publish(out_image, out_info); + pub_.publish(std::move(out_image), std::move(out_info)); } } // namespace image_proc diff --git a/image_proc/src/crop_non_zero.cpp b/image_proc/src/crop_non_zero.cpp index 1fa8ebf61..772484334 100644 --- a/image_proc/src/crop_non_zero.cpp +++ b/image_proc/src/crop_non_zero.cpp @@ -87,9 +87,9 @@ CropNonZeroNode::CropNonZeroNode(const rclcpp::NodeOptions & options) void CropNonZeroNode::imageCb(const sensor_msgs::msg::Image::ConstSharedPtr & raw_msg) { - cv_bridge::CvImagePtr cv_ptr; + cv_bridge::CvImageConstPtr cv_ptr; try { - cv_ptr = cv_bridge::toCvCopy(raw_msg); + cv_ptr = cv_bridge::toCvShare(raw_msg); } catch (cv_bridge::Exception & e) { RCLCPP_ERROR(this->get_logger(), "cv_bridge exception: %s", e.what()); return; @@ -132,7 +132,10 @@ void CropNonZeroNode::imageCb(const sensor_msgs::msg::Image::ConstSharedPtr & ra out_msg.encoding = raw_msg->encoding; out_msg.image = cv_ptr->image(r); - pub_.publish(out_msg.toImageMsg()); + auto out_image = std::make_unique(); + out_msg.toImageMsg(*out_image); + + pub_.publish(std::move(out_image)); } } // namespace image_proc diff --git a/image_proc/src/debayer.cpp b/image_proc/src/debayer.cpp index ad870a4e8..8eddbd77e 100644 --- a/image_proc/src/debayer.cpp +++ b/image_proc/src/debayer.cpp @@ -103,7 +103,7 @@ void DebayerNode::imageCb(const sensor_msgs::msg::Image::ConstSharedPtr & raw_ms // First publish to mono if needed if (pub_mono_.getNumSubscribers()) { if (sensor_msgs::image_encodings::isMono(raw_msg->encoding)) { - pub_mono_.publish(raw_msg); + pub_mono_.publish(std::move(raw_msg)); } else { if ((bit_depth != 8) && (bit_depth != 16)) { RCLCPP_WARN( @@ -113,18 +113,21 @@ void DebayerNode::imageCb(const sensor_msgs::msg::Image::ConstSharedPtr & raw_ms } else { // Use cv_bridge to convert to Mono. If a type is not supported, // it will error out there - sensor_msgs::msg::Image::SharedPtr gray_msg; + auto gray_msg = std::make_unique(); + cv_bridge::CvImagePtr cv_image; try { if (bit_depth == 8) { - gray_msg = - cv_bridge::toCvCopy(raw_msg, sensor_msgs::image_encodings::MONO8)->toImageMsg(); + cv_image = + cv_bridge::toCvCopy(raw_msg, sensor_msgs::image_encodings::MONO8); } else { - gray_msg = - cv_bridge::toCvCopy(raw_msg, sensor_msgs::image_encodings::MONO16)->toImageMsg(); + cv_image = + cv_bridge::toCvCopy(raw_msg, sensor_msgs::image_encodings::MONO16); } - pub_mono_.publish(gray_msg); + cv_image->toImageMsg(*gray_msg); + + pub_mono_.publish(std::move(gray_msg)); } catch (cv_bridge::Exception & e) { RCLCPP_WARN(this->get_logger(), "cv_bridge conversion error: '%s'", e.what()); } @@ -154,8 +157,7 @@ void DebayerNode::imageCb(const sensor_msgs::msg::Image::ConstSharedPtr & raw_ms raw_msg->height, raw_msg->width, CV_MAKETYPE(type, 1), const_cast(&raw_msg->data[0]), raw_msg->step); - sensor_msgs::msg::Image::SharedPtr color_msg = - std::make_shared(); + auto color_msg = std::make_unique(); color_msg->header = raw_msg->header; color_msg->height = raw_msg->height; color_msg->width = raw_msg->width; @@ -218,16 +220,17 @@ void DebayerNode::imageCb(const sensor_msgs::msg::Image::ConstSharedPtr & raw_ms cv::cvtColor(bayer, color, code); } - pub_color_.publish(color_msg); + pub_color_.publish(std::move(color_msg)); } else if (raw_msg->encoding == sensor_msgs::image_encodings::YUV422 || // NOLINT raw_msg->encoding == sensor_msgs::image_encodings::YUV422_YUY2) { // Use cv_bridge to convert to BGR8 - sensor_msgs::msg::Image::SharedPtr color_msg; + auto color_msg = std::make_unique(); try { - color_msg = cv_bridge::toCvCopy(raw_msg, sensor_msgs::image_encodings::BGR8)->toImageMsg(); - pub_color_.publish(color_msg); + auto cv_image = cv_bridge::toCvCopy(raw_msg, sensor_msgs::image_encodings::BGR8); + cv_image->toImageMsg(*color_msg); + pub_color_.publish(std::move(color_msg)); } catch (const cv_bridge::Exception & e) { RCLCPP_WARN(this->get_logger(), "cv_bridge conversion error: '%s'", e.what()); } diff --git a/image_proc/src/rectify.cpp b/image_proc/src/rectify.cpp index 7b0e32fb0..d1ada0c97 100644 --- a/image_proc/src/rectify.cpp +++ b/image_proc/src/rectify.cpp @@ -152,9 +152,9 @@ void RectifyNode::imageCb( model_.rectifyImage(image, rect, interpolation_); // Allocate new rectified image message - sensor_msgs::msg::Image::SharedPtr rect_msg = - cv_bridge::CvImage(image_msg->header, image_msg->encoding, rect).toImageMsg(); - pub_rect_.publish(rect_msg); + auto rect_msg = std::make_unique(); + cv_bridge::CvImage(image_msg->header, image_msg->encoding, rect).toImageMsg(*rect_msg); + pub_rect_.publish(std::move(rect_msg)); TRACEPOINT( image_proc_rectify_fini, diff --git a/image_proc/src/resize.cpp b/image_proc/src/resize.cpp index b342096ae..bca4a1d33 100644 --- a/image_proc/src/resize.cpp +++ b/image_proc/src/resize.cpp @@ -129,8 +129,7 @@ void ResizeNode::imageCb( cv::resize(cv_ptr->image, scaled_cv_.image, cv::Size(width, height), 0, 0, interpolation_); } - sensor_msgs::msg::CameraInfo::SharedPtr dst_info_msg = - std::make_shared(*info_msg); + auto dst_info_msg = std::make_unique(*info_msg); double scale_y; double scale_x; @@ -163,9 +162,13 @@ 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); + auto dst_image_msg = std::make_unique(); + scaled_cv_.header = image_msg->header; scaled_cv_.encoding = image_msg->encoding; - pub_image_.publish(*scaled_cv_.toImageMsg(), *dst_info_msg); + scaled_cv_.toImageMsg(*dst_image_msg); + + pub_image_.publish(std::move(dst_image_msg), std::move(dst_info_msg)); TRACEPOINT( image_proc_resize_fini, diff --git a/image_publisher/src/image_publisher.cpp b/image_publisher/src/image_publisher.cpp index 325c28523..44a4a514b 100644 --- a/image_publisher/src/image_publisher.cpp +++ b/image_publisher/src/image_publisher.cpp @@ -169,14 +169,16 @@ void ImagePublisher::doWork() image_flipped_ = true; } - sensor_msgs::msg::Image::SharedPtr out_img = - cv_bridge::CvImage(std_msgs::msg::Header(), "bgr8", image_).toImageMsg(); + auto out_img = std::make_unique(); + cv_bridge::CvImage(std_msgs::msg::Header(), "bgr8", image_).toImageMsg(*out_img); out_img->header.frame_id = frame_id_; out_img->header.stamp = this->now(); - camera_info_.header.frame_id = out_img->header.frame_id; - camera_info_.header.stamp = out_img->header.stamp; - pub_.publish(*out_img, camera_info_); + auto cam_info = std::make_unique(camera_info_); + cam_info->header.frame_id = out_img->header.frame_id; + cam_info->header.stamp = out_img->header.stamp; + + pub_.publish(std::move(out_img), std::move(cam_info)); } catch (cv::Exception & e) { RCLCPP_ERROR( this->get_logger(), "Image processing error: %s %s %s %i", diff --git a/image_rotate/src/image_rotate_node.cpp b/image_rotate/src/image_rotate_node.cpp index fd70e8476..6b8097cc2 100644 --- a/image_rotate/src/image_rotate_node.cpp +++ b/image_rotate/src/image_rotate_node.cpp @@ -262,10 +262,10 @@ void ImageRotateNode::do_work( cv::warpAffine(in_image, out_image, rot_matrix, cv::Size(out_size, out_size)); // Publish the image. - sensor_msgs::msg::Image::SharedPtr out_img = - cv_bridge::CvImage(msg->header, msg->encoding, out_image).toImageMsg(); + auto out_img = std::make_unique(); + cv_bridge::CvImage(msg->header, msg->encoding, out_image).toImageMsg(*out_img); out_img->header.frame_id = transform.child_frame_id; - img_pub_.publish(out_img); + img_pub_.publish(std::move(out_img)); } catch (const cv::Exception & e) { RCLCPP_ERROR( get_logger(), diff --git a/stereo_image_proc/src/stereo_image_proc/disparity_node.cpp b/stereo_image_proc/src/stereo_image_proc/disparity_node.cpp index 0a9f41945..8ccf60bca 100644 --- a/stereo_image_proc/src/stereo_image_proc/disparity_node.cpp +++ b/stereo_image_proc/src/stereo_image_proc/disparity_node.cpp @@ -353,7 +353,7 @@ void DisparityNode::imageCb( model_.fromCameraInfo(l_info_msg, r_info_msg); // Allocate new disparity image message - auto disp_msg = std::make_shared(); + auto disp_msg = std::make_unique(); disp_msg->header = l_info_msg->header; disp_msg->image.header = l_info_msg->header; @@ -384,7 +384,7 @@ void DisparityNode::imageCb( // Perform block matching to find the disparities block_matcher_.processDisparity(l_image, r_image, model_, *disp_msg); - pub_disparity_->publish(*disp_msg); + pub_disparity_->publish(std::move(disp_msg)); } rcl_interfaces::msg::SetParametersResult DisparityNode::parameterSetCb( diff --git a/stereo_image_proc/src/stereo_image_proc/point_cloud_node.cpp b/stereo_image_proc/src/stereo_image_proc/point_cloud_node.cpp index 36c3757a4..8d8715a42 100644 --- a/stereo_image_proc/src/stereo_image_proc/point_cloud_node.cpp +++ b/stereo_image_proc/src/stereo_image_proc/point_cloud_node.cpp @@ -235,7 +235,7 @@ void PointCloudNode::imageCb( cv::Mat_ mat = points_mat_; // Fill in new PointCloud2 message (2D image-like layout) - auto points_msg = std::make_shared(); + auto points_msg = std::make_unique(); points_msg->header = disp_msg->header; points_msg->height = mat.rows; points_msg->width = mat.cols; @@ -351,7 +351,7 @@ void PointCloudNode::imageCb( } } - pub_points2_->publish(*points_msg); + pub_points2_->publish(std::move(points_msg)); } } // namespace stereo_image_proc