Using sensor_msgs/PointCloud2 natively

PointCloud_to_PointCloud2

asked 2015-10-26 05:14:28 -0500

ben.gill
21 ●1 ●2 ●5

updated 2015-10-26 09:56:00 -0500

I've been working on a node that can take in data from a laser rangefinder (LIDAR Lite) mounted on a gimbal, using an encoder to measure the direction that the rangefinder is pointing. There is some uncertainty about the step angle between measurements, so using the sensor_msgs/LaserScan message type is not appropriate.

I have successfully used the sensor_msgs/PointCloud message type to produce a point cloud & display in RVIZ. I'm now finding that this message type is not compatible with mapping or conversion to LaserScans for SLAM. I would like to convert my current node to use the PointCloud2 message type. I know that there is a convertPointCloudToPointCloud2 function, but I would like to format my data correctly rather than pass it through a conversion if possible. How do I use the PointCloud2 message type to publish what effectively is an array of XYZ points?

Many Thanks,

Ben

edit retag flag offensive close merge delete

Comments

Ok, so what is your actual issue / question here?

gvdhoorn ( 2015-10-26 05:36:33 -0500 )edit

What do you mean by "using natively"? Do you just want your node to send out PointCloud2 directly without conversion nodes or do you really want to fill a PointCloud2 by hand without other code doing that for you? There is code that allows you to basically just fill a point vector and publish that.

dornhege ( 2015-10-27 04:13:44 -0500 )edit

By natively, I do mean fill out the PointCloud2 manually. I currently have a sensor passing in a range and direction, and would like to publish to a PointCloud2 in order to use the sensor as input for mapping or SLAM.

ben.gill ( 2015-10-27 04:56:41 -0500 )edit

In that case, just use pcl_ros. You can conveniently put your points in a pcl PointCloud (not PointCloud2) and publish that as a PointCloud2.

dornhege ( 2015-10-27 06:45:27 -0500 )edit

that is a good idea

Dimitri Schachmann ( 2015-10-27 06:57:30 -0500 )edit

add a comment

2 Answers

Sort by » oldest newest most voted

answered 2015-10-26 17:00:07 -0500

Dimitri Schachmann
789 ●12 ●15 ●24

updated 2015-10-27 03:42:25 -0500

Are you asking how to construct a sensor_msgs::PointCloud2 message from scratch? If this is the case: you construct it like any other ros message by first looking up the structure and documentation of that message in the .msg file.

Depending on the ros implementation you are using, most probably roscpp or rospy, you then need to construct an initialize its members. In c++ something like the following and in python also not too different.

#include <sensor_msgs/PointCloud2.h>

...

sensor_msgs::PointCloud2 msg; // construct message
msg.header = laserMsg.header; // perhaps something different, but often this is what you want
msg.height = 1; // or whatever you need
msg.widht  = 10000; // or whatever your width is
msg.fields = ... // this is more complicated but just is just more of the same
// and so on

Admittedly the sensor_msgs::PointCloud2 message is one of the more complicated messages you may use (at least for me that's true). You need to read the .msg file very carefully, which should be enough to build your own message. It could help to inspect existing PointCloud2 messages that you know to be correct and/or look into the implementation of convertPointCloudToPointCloud2 .

Please note the most important part to understand about the message: in the PointField[] fields you define the format in which your points are stored in the uint8[] data member.

I just realized myself, that PointCloud2 allows for a lot of flexibility, because of PointField[] fields, so I'm not sure what needs to be done to not break interoperability with other code. I'd assume that many nodes expect you define at least "x", "y" and "z" fields.

Edit answering the comment From what I read in the documentation

point_step is the size of one point in bytes, which you need to manually calculate for your particular format as specified in msg.fields. (with pen & paper). For example if msg.fields[0] has datatype uint16 (2 bytes) and count 3, then your write down 23=6 bytes for that field. Plus the rest of msg.fields. A common use case would be for you to have 3 entries in msg.fields (for x, y and z) where each has datatype = FLOAT32, which is 4 bytes. So point_step would be 34=12.
row_step is should be just width*point_step. Seems rather odd me, because that is redundant.
data then contains your actual data as specified in fields. So in the simple use case from above it would just be x1, y1,z1, x2, y2, z3, x3, y3, z3, ....
What "is_dense = true" means is: this dataset has _no_ invalid points (e.g., NaN, Inf). (Source is about PCL but it sure is equivalent)

edit flag offensive delete link

Comments

Thanks Dimitri, It seems that didn't ask my question that clearly. I had reviewed the msg documentation, and could understand the members that you listed. Where I'm unclear is the usage of msg.point_step, msg.row_step, msg.data[] & msg.is_dense members.

ben.gill ( 2015-10-26 22:37:26 -0500 )edit

add a comment

answered 2017-10-10 12:11:33 -0500

biglotusturtle
165 ●3 ●10 ●10

Not sure if this will help someone else but this is what I have for converting an OpenCV mat pointcloud into a sensor_msgs::PointCloud2 message

//convert point cloud image to ros message
sensor_msgs::PointCloud2 img_to_cloud(const cv::Mat& rgb, const cv::Mat &coords)
{
//rgb is a cv::Mat with 3 color channels of size 640x480
//coords is a cv::Mat with xyz channels of size 640x480, units in mm from calibration

//figure out number of points
int numpoints = rgb.size().width * rgb.size().height;

//declare message and sizes
sensor_msgs::PointCloud2 cloud;
cloud.header.frame_id = camera_frame_id;
cloud.header.stamp = ros::Time::now();
cloud.width  = numpoints;
cloud.height = 1;
cloud.is_bigendian = false;
cloud.is_dense = false; // there may be invalid points

//for fields setup
sensor_msgs::PointCloud2Modifier modifier(cloud);
modifier.setPointCloud2FieldsByString(2,"xyz","rgb");
modifier.resize(numpoints);

//iterators
sensor_msgs::PointCloud2Iterator<float> out_x(cloud, "x");
sensor_msgs::PointCloud2Iterator<float> out_y(cloud, "y");
sensor_msgs::PointCloud2Iterator<float> out_z(cloud, "z");
sensor_msgs::PointCloud2Iterator<uint8_t> out_r(cloud, "r");
sensor_msgs::PointCloud2Iterator<uint8_t> out_g(cloud, "g");
sensor_msgs::PointCloud2Iterator<uint8_t> out_b(cloud, "b");

for (int y=0; y<coords.rows; y++)
{
    for (int x=0; x<coords.cols; x++)
    {
        //get the image coordinate for this point and convert to mm
        cv::Vec3f pointcoord = coords.at<cv::Vec3f>(y, x);
        float X_World = pointcoord.val[0] * mm_to_m;
        float Y_World = pointcoord.val[1] * mm_to_m;
        float Z_World = pointcoord.val[2] * mm_to_m;

        //get the colors
        cv::Vec3b color = rgb.at<cv::Vec3b>(y,x);
        uint8_t r = (color[2]);
        uint8_t g = (color[1]);
        uint8_t b = (color[0]);

        //store xyz in point cloud, transforming from image coordinates, (Z Forward to X Forward)
        *out_x = Z_World;
        *out_y = -X_World;
        *out_z = -Y_World;

        // store colors
        *out_r = r;
        *out_g = g;
        *out_b = b;

        //increment
        ++out_x;
        ++out_y;
        ++out_z;
        ++out_r;
        ++out_g;
        ++out_b;
    }
}
return cloud;
}

Also, if you see something wrong with this code please let me know

edit flag offensive delete link

Comments

This was very helpful! Thank you.

deepaktalwardt ( 2020-06-22 16:36:58 -0500 )edit

add a comment

Using sensor_msgs/PointCloud2 natively

Comments

2 Answers

Comments

Comments

Question Tools

Stats

Using sensor_msgs/PointCloud2 natively edit

Comments

2 Answers

Comments

Comments

Question Tools

Stats

Using sensor_msgs/PointCloud2 natively