Rosanswers logo

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,


Originally posted by ben.gill on ROS Answers with karma: 21 on 2015-10-26

Post score: 2

Original comments

Comment by gvdhoorn on 2015-10-26:
Ok, so what is your actual issue / question here?

Comment by dornhege on 2015-10-27:
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.

Comment by ben.gill on 2015-10-27:
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.

Comment by dornhege on 2015-10-27:
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.

Comment by Dimitri Schachmann on 2015-10-27:
that is a good idea


2 Answers 2


Rosanswers logo

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)

Originally posted by Dimitri Schachmann with karma: 789 on 2015-10-26

This answer was ACCEPTED on the original site

Post score: 7

Original comments

Comment by ben.gill on 2015-10-26:
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.


Rosanswers logo

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);

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;

return cloud;

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

Originally posted by biglotusturtle with karma: 165 on 2017-10-10

This answer was NOT ACCEPTED on the original site

Post score: 6

Original comments

Comment by deepaktalwardt on 2020-06-22:
This was very helpful! Thank you.


Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.