Help with PointCloud2 Data

asked 2019-03-24 14:55:26 -0500

tracsat
13 ●2 ●2 ●5

updated 2019-03-24 16:19:12 -0500

Hello,

I am using a Raspberry Pi with Lubuntu 16.04, ROS Kinetic, a tim 561 and a package called sick_scan to get data from the LIDAR. I can use sick_scan to publish PointCloud2 data to the topic /cloud and I can visualize data with rviz. The end goal for my LIDAR is to be able to get distance data from it. So I want it to tell me what the distance to the things around it is. I am having some issues with this though as I am not really sure where to start. Would I be able to extract distance data from directly from the PointCloud2 data and if so how, or would there be some intermediary steps?

I have successfully followed this tutorial to convert PointCloud2 to PointCloud<t> but I'm not really sure if this is the right direction to go in. I'm also not really sure what the difference between the PointCloud2 and PointCloud<t> is. I noticed though that the data looks very different, is this by design or my mistake? (I will post a comparison below).

I also found this tutorial that changes a PointCloud to a rangeImage, would this be helpful?

I would also like to add that I am beginner and PointCloud2 is by far the most complex data type I have worked with. I am used to the simpler data types, so any help would be truly appreciated as I am sure that I am missing some key understanding about this data type.

Thanks!

/output data after converting to PointCloud<t>:

header: 
  seq: 895
  stamp: 
    secs: 1553455523
    nsecs: 598997000
  frame_id: "cloud"
values: [-0.0, -0.0, 1.0, -0.0]
---
header: 
  seq: 896
  stamp: 
    secs: 1553455523
    nsecs: 665580000
  frame_id: "cloud"
values: [-0.0, -0.0, 1.0, -0.0]

/cloud data of type PointCloud2:

header: 
  seq: 296
  stamp: 
    secs: 1553456947
    nsecs: 421859979
  frame_id: "cloud"
height: 1
width: 811
fields: 
  - 
    name: "x"
    offset: 0
    datatype: 7
    count: 1
  - 
    name: "y"
    offset: 4
    datatype: 7
    count: 1
  - 
    name: "z"
    offset: 8
    datatype: 7
    count: 1
  - 
    name: "intensity"
    offset: 12
    datatype: 7
    count: 1
is_bigendian: False
point_step: 16
row_step: 12976
data: [108, 171, 8, 191, 107, 171, 8, 191, 0, 0, 0, 0, 0, 0, 113, 70, 167, 197, 8, 191, 103, 95, 10, 191, 0, 0, 0, 0, 0, 240, 115, 70, 99, 101, 9, 191, 127, 161, 12, 191, 0, 0, 0, 0, 0, 200, 106, 70, 99, 50, 5, 191, 202, 237, 9, 191, 0, 0, 0, 0, 0, 76, 82, 70, 200, 22, 235, 190, 200, 74, 246, 190, 0, 0, 0, 0, 0, 132, 52, 70, 186, 111, 255, 190, 99, 95, 7, 191, 0, 0, 0, 0, 0, 24, 60, 70, 227, 1, 8, 191, 89, 217, 17, 191, 0, 0, 0, 0, 0, 64, 93, 70, 216, 183, 8, 191, 245, 84, 20, 191, 0, 0, 0, 0, 0, 236, 112, 70, 195, 94, 8, 191, 64, 177, 21, 191, 0, 0, 0, 0 ...

(more)

answered 2019-03-24 20:21:06 -0500

janindu

849 ●7 ●26 ●19

The actual measurement of a laser scanner is the distance. So ideally you should be able to go through the code of the driver and publish that data into a custom message. Performance wise, that is the best option.

However, let me give you a simpler solution.

For your reference, this is the PointCloud2 message definition. Essentially, the large array data contains x,y,z location of the points in the point cloud. All you need to do is to parse the data array, obtain x,y,z for each point and then calculate the euclidean distance where (0,0,0) is the location of the sensor / frame.

The parsing logic is simple. Your message has height 1, width 811 and point_step 16. You can verify the numbers by checking

width*point_step == row_step*height

The fields give you information about parsing the data. Each group of 16 numbers in the array data gives you x,y,z and intensity for a single point in the point cloud. First 1-4 numbers represent x as a 32 bit float, 5-8 represents y, 9-12 represents z, 13-16 represents the intensity. All you then need to do is convert the data to 32 bit float in little endian.

Libraries should exist where all this is done. However, if you are into reinventing the wheel or couldn't find one, this will get you home.

edit flag offensive delete link

Comments

Ah ok this makes a lot of sense thank you! I had come to a similar conclusion after reading through a bunch of blog posts but I was getting stuck on the float32 part - I was not aware that a float32 could be represented this way. Thank you for the link for converting to a float32. I notice the solution that was taken says that it assumes that the data is not little endian, however I believe my data is. Is there a way around this? This is my first time encountering endian so I am not sure how much of an issue it may be. Thanks again!

tracsat ( 2019-03-24 20:36:01 -0500 )edit

In little endian, the bytes are ordered in reverse compared to big endian. So all you have to do is to follow the same implementation but use the bytes in reverse order.

janindu ( 2019-03-24 21:00:12 -0500 )edit

Ok that makes a lot of sense thank you! I just have one more thing if you don't mind. I noticed that the solution uses a special datatype they have to define 'uchar'. Would I also need this? Or would a simple int type work? Thanks

tracsat ( 2019-03-24 21:37:53 -0500 )edit

uchar is simply unsigned char type data. Even though the name says char it's essentially a numeric variable. Its size is 8 bits so can take values from 0-255. int, on the other hand, is 32 bits in size at least. So you can't represent 4 bytes (you can, but that's a headache) using int variable. unsigned char is a fundamental data type in C++ so I don't see any reason not to use it. Ref : Data Types in C++

janindu ( 2019-03-24 21:44:25 -0500 )edit

Thanks that makes a lot of sense. I was not aware that uchar was a fundamental type.

tracsat ( 2019-03-24 21:57:40 -0500 )edit