Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Publish using unique ptr #1016

Merged
merged 6 commits into from
Aug 20, 2024
Merged

Publish using unique ptr #1016

Show file tree
Hide file tree
Changes from 5 commits
Commits
Show all changes
6 commits
Select commit Hold shift + click to select a range
5eead4f
Publish using unique_ptr in image_proc
bjsowa Aug 1, 2024
45ff6a4
Publish using unique_ptr in depth_image_proc
bjsowa Aug 1, 2024
0142ed3
Publish using unique_ptr in image_publisher
bjsowa Aug 1, 2024
c03ea44
Publish using unique_ptr in image_rotate
bjsowa Aug 1, 2024
2f4693b
Publish using unique_ptr in stereo_image_proc
bjsowa Aug 1, 2024
4d18545
Merge remote-tracking branch 'origin/rolling' into publish-using-uniq…
bjsowa Aug 19, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
34 changes: 17 additions & 17 deletions depth_image_proc/include/depth_image_proc/conversions.hpp
View file
Open in desktop
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,
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
Open in desktop
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
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -99,7 +99,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 @@ -136,7 +136,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
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -144,7 +144,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
Open in desktop
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 @@ -127,7 +127,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 @@ -144,30 +144,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
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -97,7 +97,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 @@ -112,16 +112,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
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -92,7 +92,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 @@ -114,16 +114,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
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -187,7 +187,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 @@ -205,9 +205,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 @@ -216,21 +216,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
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -115,7 +115,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 @@ -143,31 +143,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