Problems reading PointCloud2 message

edit

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