Ask Your Question
0

Problems reading PointCloud2 message

asked 2017-12-21 15:48:19 -0600

Null Terminator gravatar image

Hello everyone, I'm trying to find an object by color and then get the distance to it. However I'm having trouble reading the PointCloud2 message.

I can see through RVIZ that the topic is sending the right cloud, however when I try to read it through my code I am having some issues:

  • The x,y,z are sometimes negative
  • The rgb values are almost zero (6.82247660989e-39 for example).

My code for getting the RGB values of the cloud:

generator = pc2.read_points(cloud, field_names = ['rgb'], skip_nans = True)

matrix = np.empty(shape = (cloud.height, cloud.width), dtype=float)
for i, element in enumerate(generator):
    print element[0] // for debugging reasons, prints a near-zero float
    np.put(matrix, [[i / cloud.width, i % cloud.width]], [element[0]])

Note that the variable cloud is the PointCloud2 message recieved from the topic, and pc2 is the namespace PointCloud2.

What am I doing wrong, and how can I get a matrix to use with OpenCV2?

I'm using ROS Indigo, running through Gazebo. Thanks in advance

edit retag flag offensive close merge delete

1 Answer

Sort by ยป oldest newest most voted
0

answered 2017-12-22 10:02:31 -0600

Null Terminator gravatar image

I managed to solve the bgr issue thanks to the lovely topic here: link text

For future readers: All 3 colors are merged in a same float (binary-wise) and they require some bitwise operations to read. My code is as follows:

def on_new_img(msg):
    print 'got something!!'

    bgr = cloud2bgr(msg)

    cv2.imshow("img", bgr)
    cv2.waitKey(0)

def bgr_gen(sequence):
    for element in sequence:
        #convert to int so bitwise operations become enabled
        s = struct.pack('>f' ,element[0])
        i = struct.unpack('>l',s)[0]

        pack = ctypes.c_uint32(i).value
        r = (pack & 0x00FF0000)>> 16
        g = (pack & 0x0000FF00)>> 8
        b = (pack & 0x000000FF)

        yield (float(b) /255, float(g) / 255, float(r) / 255)

def cloud2bgr(cloud):
    generator = pc2.read_points(cloud, field_names = ['rgb'], skip_nans = True)
    bgr_generator = bgr_gen(generator)

    matrix = np.empty(shape = (cloud.height, cloud.width, 3), dtype=float)

    for row in range(cloud.height):
        for col in range(cloud.width):
            matrix[row][col] = bgr_generator.next()

    return matrix
edit flag offensive delete link more

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

2 followers

Stats

Asked: 2017-12-21 15:48:19 -0600

Seen: 199 times

Last updated: Dec 21 '17