Getting weird values when doing a rostopic echo on a PointCloud2

asked 2016-03-29 08:55:14 -0600

badrobit gravatar image

We are currently working on writing a ROS driver for a new LiDAR system. When we read the values that our sensor provides they make sense as in we get something that would look like this:

Point: 
    X: 1.0000
    Y: -0.4500
    Z: 2.90000
    I: 36

but when we try to pack them into a sensor_msg::PointCloud2 when we do a rostopic echo /lidar_driver/points we see the following:

Point: 
    X: 000
    Y: 123
    Z: 000
    I: 36

Note: The values I put in are made up but are meant to illustrate the point, and while the number are fake in the rostopic echo we never see anymore than 3 values show up in the output.

When we convert from our drivers point cloud representation to the ROS sensor_msgs::PointCloud2 we do it like this:

sensor_msgs::PointCloud2 msgOut;
msgOut.header.stamp = ros::Time::now();
msgOut.header.seq = m_messageSeqNum;
msgOut.header.frame_id = "lidar_driver";
m_messageSeqNum++;

msgOut.fields.clear();

// Describe X field
sensor_msgs::PointField fieldX;
fieldX.name = "X";
fieldX.datatype = sensor_msgs::PointField::FLOAT64;
fieldX.count = vecPts.size();
fieldX.offset = 0;
msgOut.fields.push_back(fieldX);

// Describe Y field
sensor_msgs::PointField fieldY;
fieldY.name = "Y";
fieldY.datatype = sensor_msgs::PointField::FLOAT64;
fieldY.count = vecPts.size();
fieldY.offset =  sizeof( double );
msgOut.fields.push_back(fieldY);

// Describe Z field
sensor_msgs::PointField fieldZ;
fieldZ.name = "Z";
fieldZ.datatype = sensor_msgs::PointField::FLOAT64;
fieldZ.count = vecPts.size();
fieldZ.offset = 2 * sizeof( double );
msgOut.fields.push_back(fieldZ);

// Describe timestamp field
sensor_msgs::PointField fieldTimestamp;
fieldTimestamp.name = "timestamp";
fieldTimestamp.datatype = sensor_msgs::PointField::FLOAT64; // timestamp will be converted to double to match ROS format
fieldTimestamp.count = vecPts.size();
fieldTimestamp.offset = 3 * sizeof( double );
msgOut.fields.push_back(fieldTimestamp);

// Describe intensity field
sensor_msgs::PointField fieldIntensity;
fieldIntensity.name = "intensity";
fieldIntensity.datatype = sensor_msgs::PointField::UINT8;
fieldIntensity.count = vecPts.size();
fieldIntensity.offset = 4 * sizeof( double );
msgOut.fields.push_back(fieldIntensity);

msgOut.height = 1;
msgOut.width = vecPts.size();
msgOut.is_bigendian = false;
msgOut.point_step = sizeof( double ) * 4 + sizeof( uint8_t ); 
msgOut.row_step = msgOut.point_step * msgOut.width;
msgOut.data.clear();
msgOut.data.resize(msgOut.row_step, 0);
unsigned int uiPos = 0;

std::cout << "New PointCloud" << std::endl; 
for( lidar_driver::PointsVector::iterator it = vecPts.begin(); it != vecPts.end(); ++it )
{
    std::cout << "Point:" << std::endl; 

    msgOut.data[uiPos] = static_cast<double>((*it).X);
    std::cout << "\tX:" << (double)(msgOut.data[uiPos]) << std::endl; 

    uiPos += sizeof(double);
    msgOut.data[uiPos] = static_cast<double>((*it).Y);
    std::cout << "\tY:" << (double)(msgOut.data[uiPos]) << std::endl; 

    uiPos += sizeof(double);
    msgOut.data[uiPos] = static_cast<double>((*it).Z);
    std::cout << "\tZ:" << (double)(msgOut.data[uiPos]) << std::endl; 

    uiPos += sizeof(double);
    double* pTimestamp = reinterpret_cast<double*>(&msgOut.data[uiPos]);
    *pTimestamp = static_cast<double>((*it).timestamp) / 1000000.0;

    uiPos += sizeof(double);
    uint8_t *pIntensity = reinterpret_cast<uint8_t*>(&msgOut.data[uiPos]);
    *pIntensity = static_cast<uint8_t>((*it).intensity);

    std::cout << "\tI:" << (unsigned)*pIntensity << "\t\t\t" << (unsigned)(msgOut.data[uiPos]) << "\n" << std::endl; 

    uiPos += sizeof(uint8_t);
}

m_pointsPublisher.publish(msgOut);

my best guess as to what the problem is is that is has something to do with us storing FLOAT64 values in a uint8_t array that makes up the sensor_msgs::PointCloud2.data field.

Any help ... (more)

edit retag flag offensive close merge delete