CODEWITHSUNDEEP
c++rospoint-cloud-library
YJMoon
YJMoon

Reputation: 3

How to convert (ROS) "geometry_msgs/Point" to sensor_msgs/PointCloud2?

I'm a student studying LIDAR algorithm. I have a LIDAR code that subscribes to sensor_msgs/PointCloud2. and I'm receiving geometry_msgs/Point data now. I wanna convert geometry_msgs/Point to sensor_msgs/PointCloud2. And I wanna apply the code I wrote. Please tell me the sensor_msgs::PointCloud2 type function in c++ !! And there are width and height, and so on in sensor_msgs/PointCloud2. How do I convert it? I'm curious because geometry_msgs/Point doesn't have them.

If it's sensor_msgs/LaserScan, I've converted it, but I'm not sure about geometry_msgs/Point.

Upvotes: 0

Views: 1489

Answers (2)

kosmastsk
kosmastsk

Reputation: 69

The simplest way to do this is to create a pcl PointCloud2 struct, fill it with the geometry_msgs/Point data and adapt width accordingly. If you have a vector points of type geometry_msgs/Point you can do the following.

pcl::PointCloud<pcl::PointXYZ>::Ptr tmp(new pcl::PointCloud<pcl::PointXYZ>);
tmp->width = 0;
tmp->height = 1;
tmp->is_dense = false;
tmp->points.resize(tmp->width * tmp->height);
tmp->header.frame_id = _your_frame;

for (int i = 0; i < points.size(); i++)
{
    pcl::PointXYZ pt;
    pt.x = points.at(i).x;
    pt.y = points.at(i).y;
    pt.z = points.at(i).z;
    tmp->points.push_back(pt);
    tmp->width++;
}

And now you can convert this PCL Pointcloud to Pointcloud2 using the known methods.

Upvotes: 1

ignacio
ignacio

Reputation: 1197

I think the easiest way is to create a sensor_msgs::PointCloud, which contains a vector of geometry_msgs::Point32.

Then add your points to this msg and you call the method convertPointCloudToPointCloud2from point_cloud_conversion.hpp

static inline bool convertPointCloudToPointCloud2(
  const sensor_msgs::msg::PointCloud & input,
  sensor_msgs::msg::PointCloud2 & output)

to get the sensor_msgs::PointCloud2

or you could try creating a pcl::PointCloud2 and then convert using either the method fromPCL or moveFromPCL from pcl_conversions

Upvotes: 0

Related Questions