Accessing depth data from a pointcloud2 message
Hi all!
I want to select the depth (z) values of a given row from a sensor_msgs/pointcloud2
message. I have this code so far:
for (int i = 0; i < cloud->width; i++)
{
int index = cloud->row_step*ROW_NUM + cloud->point_step*i + cloud->fields[2].offset;
memcpy(&depth_data[i], &cloud->data[index], sizeof(float));
}
However the code is probably not correct, as the values it gives don't seem to be the real ones. I chose the middle row of the pointcloud (num. 239) and put the sensor in front of the wall, so "the ray of the middle row" should be normal to the wall and so the z value should be the real distance from the wall. The result was: real distance 2m, the z value from the message 0.001. When I changed the row number to some other value, the z values were even below 0 (which is definitely not correct as I want to get the forward distances). So what am I doing wrong in my code? (I do not want to convert the ros message to a pcl structure.) Thanks in advance!