Publish using unique ptr (#1016)

Prevents doing an extra copy of the data when using intra-process
communication.

depth_image_proc/include/depth_image_proc/conversions.hpp

@@ -51,7 +51,7 @@ namespace depth_image_proc
 template<typename T>
 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<T>::fromMeters(invalid_depth);
  }
- sensor_msgs::PointCloud2Iterator<float> iter_x(*cloud_msg, "x");
- sensor_msgs::PointCloud2Iterator<float> iter_y(*cloud_msg, "y");
- sensor_msgs::PointCloud2Iterator<float> iter_z(*cloud_msg, "z");
+ sensor_msgs::PointCloud2Iterator<float> iter_x(cloud_msg, "x");
+ sensor_msgs::PointCloud2Iterator<float> iter_y(cloud_msg, "y");
+ sensor_msgs::PointCloud2Iterator<float> iter_z(cloud_msg, "z");
 
  const T * depth_row = reinterpret_cast<const T *>(&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<typename T>
 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<float>::quiet_NaN();
 
- sensor_msgs::PointCloud2Iterator<float> iter_x(*cloud_msg, "x");
- sensor_msgs::PointCloud2Iterator<float> iter_y(*cloud_msg, "y");
- sensor_msgs::PointCloud2Iterator<float> iter_z(*cloud_msg, "z");
+ sensor_msgs::PointCloud2Iterator<float> iter_x(cloud_msg, "x");
+ sensor_msgs::PointCloud2Iterator<float> iter_y(cloud_msg, "y");
+ sensor_msgs::PointCloud2Iterator<float> iter_z(cloud_msg, "z");
  const T * depth_row = reinterpret_cast<const T *>(&depth_msg->data[0]);
  int row_step = depth_msg->step / sizeof(T);
- for (int v = 0; v < static_cast<int>(cloud_msg->height); ++v, depth_row += row_step) {
- for (int u = 0; u < static_cast<int>(cloud_msg->width); ++u, ++iter_x, ++iter_y, ++iter_z) {
+ for (int v = 0; v < static_cast<int>(cloud_msg.height); ++v, depth_row += row_step) {
+ for (int u = 0; u < static_cast<int>(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<typename T>
 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<float> iter_i(*cloud_msg, "intensity");
+ sensor_msgs::PointCloud2Iterator<float> iter_i(cloud_msg, "intensity");
  const T * inten_row = reinterpret_cast<const T *>(&intensity_msg->data[0]);
 
  const int i_row_step = intensity_msg->step / sizeof(T);
- for (int v = 0; v < static_cast<int>(cloud_msg->height); ++v, inten_row += i_row_step) {
- for (int u = 0; u < static_cast<int>(cloud_msg->width); ++u, ++iter_i) {
+ for (int v = 0; v < static_cast<int>(cloud_msg.height); ++v, inten_row += i_row_step) {
+ for (int u = 0; u < static_cast<int>(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);

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<uint8_t> iter_r(*cloud_msg, "r");
- sensor_msgs::PointCloud2Iterator<uint8_t> iter_g(*cloud_msg, "g");
- sensor_msgs::PointCloud2Iterator<uint8_t> iter_b(*cloud_msg, "b");
+ sensor_msgs::PointCloud2Iterator<uint8_t> iter_r(cloud_msg, "r");
+ sensor_msgs::PointCloud2Iterator<uint8_t> iter_g(cloud_msg, "g");
+ sensor_msgs::PointCloud2Iterator<uint8_t> 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<int>(cloud_msg->height); ++v, rgb += rgb_skip) {
- for (int u = 0; u < static_cast<int>(cloud_msg->width); ++u,
+ for (int v = 0; v < static_cast<int>(cloud_msg.height); ++v, rgb += rgb_skip) {
+ for (int u = 0; u < static_cast<int>(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<uint16_t>(
  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);
 

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<sensor_msgs::msg::Image>();
+ auto depth_msg = std::make_unique<sensor_msgs::msg::Image>();
  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

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<sensor_msgs::msg::Image>();
+ cv_ptr->toImageMsg(*image_msg);
+ pub_depth_.publish(std::move(image_msg));
 }
 
 } // namespace depth_image_proc

depth_image_proc/src/disparity.cpp

@@ -77,7 +77,7 @@ class DisparityNode : public rclcpp::Node
  template<typename T>
  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<DisparityImage>();
+ auto disp_msg = std::make_unique<DisparityImage>();
  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<uint16_t>(depth_msg, disp_msg);
+ convert<uint16_t>(depth_msg, *disp_msg);
  } else if (depth_msg->encoding == sensor_msgs::image_encodings::TYPE_32FC1) {
- convert<float>(depth_msg, disp_msg);
+ convert<float>(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<typename T>
 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<T>::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<const T *>(&depth_msg->data[0]);
  int row_step = depth_msg->step / sizeof(T);
- float * disp_data = reinterpret_cast<float *>(&disp_msg->image.data[0]);
+ float * disp_data = reinterpret_cast<float *>(&disp_msg.image.data[0]);
  for (int v = 0; v < static_cast<int>(depth_msg->height); ++v) {
  for (int u = 0; u < static_cast<int>(depth_msg->width); ++u) {
  T depth = depth_row[u];

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<PointCloud2>();
+ auto cloud_msg = std::make_unique<PointCloud2>();
  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<uint16_t>(depth_msg, cloud_msg, model_, invalid_depth_);
+ convertDepth<uint16_t>(depth_msg, *cloud_msg, model_, invalid_depth_);
  } else if (depth_msg->encoding == enc::TYPE_32FC1) {
- convertDepth<float>(depth_msg, cloud_msg, model_, invalid_depth_);
+ convertDepth<float>(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

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<PointCloud>();
+ auto cloud_msg = std::make_unique<PointCloud>();
  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<uint16_t>(depth_msg, cloud_msg, transform_);
+ convertDepthRadial<uint16_t>(depth_msg, *cloud_msg, transform_);
  } else if (depth_msg->encoding == sensor_msgs::image_encodings::TYPE_32FC1) {
- convertDepthRadial<float>(depth_msg, cloud_msg, transform_);
+ convertDepthRadial<float>(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

depth_image_proc/src/point_cloud_xyzi.cpp

@@ -190,7 +190,7 @@ void PointCloudXyziNode::imageCb(
  }
  }
 
- auto cloud_msg = std::make_shared<PointCloud>();
+ auto cloud_msg = std::make_unique<PointCloud>();
  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<uint16_t>(depth_msg, cloud_msg, model_, invalid_depth_);
+ convertDepth<uint16_t>(depth_msg, *cloud_msg, model_, invalid_depth_);
  } else if (depth_msg->encoding == enc::TYPE_32FC1) {
- convertDepth<float>(depth_msg, cloud_msg, model_, invalid_depth_);
+ convertDepth<float>(depth_msg, *cloud_msg, model_, invalid_depth_);
  } else {
  RCLCPP_ERROR(
  get_logger(), "Depth image has unsupported encoding [%s]", depth_msg->encoding.c_str());
@@ -219,21 +219,21 @@ void PointCloudXyziNode::imageCb(
 
  // Convert Intensity Image to Pointcloud
  if (intensity_msg->encoding == enc::MONO8) {
- convertIntensity<uint8_t>(intensity_msg, cloud_msg);
+ convertIntensity<uint8_t>(intensity_msg, *cloud_msg);
  } else if (intensity_msg->encoding == enc::MONO16) {
- convertIntensity<uint16_t>(intensity_msg, cloud_msg);
+ convertIntensity<uint16_t>(intensity_msg, *cloud_msg);
  } else if (intensity_msg->encoding == enc::TYPE_16UC1) {
- convertIntensity<uint16_t>(intensity_msg, cloud_msg);
+ convertIntensity<uint16_t>(intensity_msg, *cloud_msg);
  } else if (intensity_msg->encoding == enc::TYPE_32FC1) {
- convertIntensity<float>(intensity_msg, cloud_msg);
+ convertIntensity<float>(intensity_msg, *cloud_msg);
  } else {
  RCLCPP_ERROR(
  get_logger(), "Intensity image has unsupported encoding [%s]",
  intensity_msg->encoding.c_str());
  return;
  }
 
- pub_point_cloud_->publish(*cloud_msg);
+ pub_point_cloud_->publish(std::move(cloud_msg));
 }
 
 

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<PointCloud>();
+ auto cloud_msg = std::make_unique<PointCloud>();
  cloud_msg->header = depth_msg->header;
  cloud_msg->height = depth_msg->height;
  cloud_msg->width = depth_msg->width;
@@ -151,31 +151,31 @@ void PointCloudXyziRadialNode::imageCb(
 
  // Convert Depth Image to Pointcloud
  if (depth_msg->encoding == sensor_msgs::image_encodings::TYPE_16UC1) {
- convertDepthRadial<uint16_t>(depth_msg, cloud_msg, transform_);
+ convertDepthRadial<uint16_t>(depth_msg, *cloud_msg, transform_);
  } else if (depth_msg->encoding == sensor_msgs::image_encodings::TYPE_32FC1) {
- convertDepthRadial<float>(depth_msg, cloud_msg, transform_);
+ convertDepthRadial<float>(depth_msg, *cloud_msg, transform_);
  } else {
  RCLCPP_ERROR(
  get_logger(), "Depth image has unsupported encoding [%s]", depth_msg->encoding.c_str());
  return;
  }
 
  if (intensity_msg->encoding == sensor_msgs::image_encodings::MONO8) {
- convertIntensity<uint8_t>(intensity_msg, cloud_msg);
+ convertIntensity<uint8_t>(intensity_msg, *cloud_msg);
  } else if (intensity_msg->encoding == sensor_msgs::image_encodings::MONO16) {
- convertIntensity<uint16_t>(intensity_msg, cloud_msg);
+ convertIntensity<uint16_t>(intensity_msg, *cloud_msg);
  } else if (intensity_msg->encoding == sensor_msgs::image_encodings::TYPE_16UC1) {
- convertIntensity<uint16_t>(intensity_msg, cloud_msg);
+ convertIntensity<uint16_t>(intensity_msg, *cloud_msg);
  } else if (intensity_msg->encoding == sensor_msgs::image_encodings::TYPE_32FC1) {
- convertIntensity<float>(intensity_msg, cloud_msg);
+ convertIntensity<float>(intensity_msg, *cloud_msg);
  } else {
  RCLCPP_ERROR(
  get_logger(), "Intensity image has unsupported encoding [%s]",
  intensity_msg->encoding.c_str());
  return;
  }
 
- pub_point_cloud_->publish(*cloud_msg);
+ pub_point_cloud_->publish(std::move(cloud_msg));
 }
 
 } // namespace depth_image_proc