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((*it).X);
std::cout << "\tX:" << (double)(msgOut.data[uiPos]) << std::endl;
uiPos += sizeof(double);
msgOut.data[uiPos] = static_cast((*it).Y);
std::cout << "\tY:" << (double)(msgOut.data[uiPos]) << std::endl;
uiPos += sizeof(double);
msgOut.data[uiPos] = static_cast((*it).Z);
std::cout << "\tZ:" << (double)(msgOut.data[uiPos]) << std::endl;
uiPos += sizeof(double);
double* pTimestamp = reinterpret_cast(&msgOut.data[uiPos]);
*pTimestamp = static_cast((*it).timestamp) / 1000000.0;
uiPos += sizeof(double);
uint8_t *pIntensity = reinterpret_cast(&msgOut.data[uiPos]);
*pIntensity = static_cast((*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 that could be provided would be appreciated!
↧