PointCloud2 Packs Bits Incorrectly

[DLu]

The Python library for writing point clouds has some weird bit-packing issues.

Consider writing this script for writing colors:

from sensor_msgs.point_cloud2 import create_cloud
from sensor_msgs.msg import PointField
import struct

fields = []
fields.append( PointField('rgb', 0, PointField.FLOAT32, 1) )

def pack_bytes(r,g,b,a):
    print 'original:', [b,g,r,a]
    x = (a << 24) + (r << 16) + (g << 8) + b
    a = struct.unpack('f', struct.pack('I', x))[0]

    points = [ (a, )]

    nc = create_cloud(None, fields, points)

    print 'packed  :', [ord(x) for x in nc.data]
    print


pack_bytes(120,0,0,0)
pack_bytes(120,0,0,255)

pack_bytes(130,0,0,0)
pack_bytes(130,0,0,255)

This outputs:

original: [0, 0, 120, 0]
packed  : [0, 0, 120, 0]

original: [0, 0, 120, 255]
packed  : [0, 0, 120, 255]

original: [0, 0, 130, 0]
packed  : [0, 0, 130, 0]

original: [0, 0, 130, 255]
packed  : [0, 0, 194, 255]

There is a bug where if red value is over 127 and the alpha value is 255, the output values do not match the input values.

I realize that the alpha is not necessarily supported, but I found it in a bag file (from a kinect I believe) and it was causing problems.