How to initialize Pointcloud2 message for XYZRGBA data?

asked 2015-04-27 13:52:23 -0600

Myzhar gravatar image

Hi all,

I'm writing a driver for an RGB-D camera and I do not want to use PCL library.

I successfully published simple pointcloud, now I'm trying to publish a RGB pointcloud, but I get an ASSERT error when I try to publish the message.

Can someone tell me if the initialization code for the message is right? I cannot find the correct fields and field names for this kind of message...

This is my initialization code

    _lastPtCloudMsgHeader.stamp = ros::Time::now();
    _lastPtCloudMsgHeader.seq = info->totalSampleCount;
    _lastPtCloudMsgHeader.frame_id = "depth_frame";

    _lastRGBPtCloud.header = _lastPtCloudMsgHeader;

    int ptsCount = info->width * info->height;

    _lastRGBPtCloud.fields.resize(4);
    _lastRGBPtCloud.fields[0].name = "z";
    _lastRGBPtCloud.fields[0].offset = 0;
    _lastRGBPtCloud.fields[0].datatype = sensor_msgs::PointField::FLOAT32;
    _lastRGBPtCloud.fields[0].count = ptsCount;
    _lastRGBPtCloud.fields[1].name = "y";
    _lastRGBPtCloud.fields[1].offset = sizeof(float);
    _lastRGBPtCloud.fields[1].datatype = sensor_msgs::PointField::FLOAT32;
    _lastRGBPtCloud.fields[1].count = ptsCount;
    _lastRGBPtCloud.fields[2].name = "x";
    _lastRGBPtCloud.fields[2].offset = 2*sizeof(float);
    _lastRGBPtCloud.fields[2].datatype = sensor_msgs::PointField::FLOAT32;
    _lastRGBPtCloud.fields[2].count = ptsCount;
    _lastRGBPtCloud.fields[3].name = "rgba";
    _lastRGBPtCloud.fields[3].offset = 3*sizeof(float);
    _lastRGBPtCloud.fields[3].datatype = sensor_msgs::PointField::FLOAT32;
    _lastRGBPtCloud.fields[3].count = ptsCount;

    _lastRGBPtCloud.width = info->width;
    _lastRGBPtCloud.height = info->height;
    _lastRGBPtCloud.is_dense = true;
    _lastRGBPtCloud.is_bigendian = false;
    _lastRGBPtCloud.point_step = 4;
    _lastRGBPtCloud.row_step = _lastRGBPtCloud.point_step * _lastRGBPtCloud.width;
edit retag flag offensive close merge delete