ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange |
1 | initial version |
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