Skip to content

Commit

Permalink
Publish using unique ptr (#1016)
Browse files Browse the repository at this point in the history 
Prevents doing an extra copy of the data when using intra-process
communication.
  • Loading branch information
@bjsowa
bjsowa authored Aug 20, 2024
1 parent d272c4e commit e5e6026
Show file tree
Hide file tree
Showing 21 changed files with 145 additions and 132 deletions.
34 changes: 17 additions & 17 deletions depth_image_proc/include/depth_image_proc/conversions.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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)
{
Expand All @@ -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
Expand All @@ -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
Expand All @@ -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];
}
}
Expand All @@ -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);
Expand Down
14 changes: 7 additions & 7 deletions depth_image_proc/src/conversions.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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];
Expand All @@ -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);

Expand Down
4 changes: 2 additions & 2 deletions depth_image_proc/src/convert_metric.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down Expand Up @@ -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
Expand Down
4 changes: 3 additions & 1 deletion depth_image_proc/src/crop_foremost.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
16 changes: 8 additions & 8 deletions depth_image_proc/src/disparity.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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)
Expand Down Expand Up @@ -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;
Expand All @@ -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];
Expand Down
8 changes: 4 additions & 4 deletions depth_image_proc/src/point_cloud_xyz.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand All @@ -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
Expand Down
8 changes: 4 additions & 4 deletions depth_image_proc/src/point_cloud_xyz_radial.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand All @@ -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
Expand Down
16 changes: 8 additions & 8 deletions depth_image_proc/src/point_cloud_xyzi.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand All @@ -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());
Expand All @@ -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));
}


Expand Down
16 changes: 8 additions & 8 deletions depth_image_proc/src/point_cloud_xyzi_radial.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down Expand Up @@ -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
Expand Down
Loading

0 comments on commit e5e6026

Please sign in to comment.