Ask Your Question
1

Understanding the bytes in a pcl2 message

asked 2018-04-24 13:15:16 -0600

Mehdi. gravatar image

I am using python in order to publish pointclouds. Prior to this I'd like to run some segmentation using python-pcl and colorize each cluster. Unfortunately, the only function that creates a pcl message from a numpy array does so without considering color (only x, y, z).

pcl2.create_cloud_xyz32(header, nx3_numpy_array)

Therefore, I want to create a ros message then tweak it manually to change the color of each point. Here is an example message:

header: 
  seq: 1535
  stamp: 
    secs: 1524593497
    nsecs: 322609179
  frame_id: "gripper_depth_camera_rgb_optical_frame"
height: 480
width: 640
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: "rgb"
    offset: 16
    datatype: 7
    count: 1
is_bigendian: False
point_step: 32
row_step: 20480
data: [0, 0, 192, 127, 0, 0, 192, 127, 0, 0, 192, 127, 0, 0, 0, 0, 0, 0, 0, 255, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0

It is easy to understand why x, y and z require 4 bytes each (float 32)

But why does rgb come at 16 and not 12? following the logic, z also needs 4 bytes, and the next free spot is at an offset of 12 and not 16.

Also why does rgb need 16 bytes? (point_step is 32 so one point requires 32 bytes from which 16 are for rgb). The color values themselves need only one byte each so this is confusing. Also, if we say the are float32 values, 16 bytes means 4 values, is the fourth value the transparency (RGBA?)

Here is an example from an XYZ only pointcloud where my understanding fits:

header: 
  seq: 5
  stamp: 
    secs: 1524583199
    nsecs: 372246980
  frame_id: "gripper_depth_camera_rgb_optical_frame"
height: 1
width: 231
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
is_bigendian: False
point_step: 12
row_step: 2772
data: [125, 207, 94, 190, 220, 24, 98, 190, 254, 67, 60, 63, 69, 231, 92, 190, 6
edit retag flag offensive close merge delete

1 Answer

Sort by ยป oldest newest most voted
2

answered 2018-04-24 17:38:07 -0600

lucasw gravatar image

updated 2018-04-24 17:45:46 -0600

Your datatype for rgb is wrong, it should be like this- the 7 is a 32 bit floating point type, this 6 is a 32 bit unsigned int:

  ...
  - 
    name: "rgb"
    offset: 12
    datatype: 6
    count: 1
is_bigendian: False
point_step: 16
...

You don't need to do it manually though, sensor_msgs.point_cloud2 create_cloud will help out (though the packing of the rgb bytes maybe could be better?):

#!/usr/bin/env python
# PointCloud2 color cube
import rospy
import struct

from sensor_msgs import point_cloud2
from sensor_msgs.msg import PointCloud2, PointField
from std_msgs.msg import Header


rospy.init_node("create_cloud_xyzrgb")
pub = rospy.Publisher("point_cloud2", PointCloud2, queue_size=2)

points = []
lim = 8
for i in range(lim):
    for j in range(lim):
        for k in range(lim):
            x = float(i) / lim
            y = float(j) / lim
            z = float(k) / lim
            pt = [x, y, z, 0]
            r = int(x * 255.0)
            g = int(y * 255.0)
            b = int(z * 255.0)
            a = 255
            print r, g, b, a
            rgb = struct.unpack('I', struct.pack('BBBB', b, g, r, a))[0]
            print hex(rgb)
            pt[3] = rgb
            points.append(pt)

fields = [PointField('x', 0, PointField.FLOAT32, 1),
          PointField('y', 4, PointField.FLOAT32, 1),
          PointField('z', 8, PointField.FLOAT32, 1),
          PointField('rgb', 12, PointField.UINT32, 1),
          # PointField('rgba', 12, PointField.UINT32, 1),
          ]

header = Header()
header.stamp = rospy.Time.now()
header.frame_id = "map"
pc2 = point_cloud2.create_cloud(header, fields, points)
while not rospy.is_shutdown():
    pc2.header.stamp = rospy.Time.now()
    pub.publish(pc2)
    rospy.sleep(1.0)

The alpha value goes along for the ride for free above, unless rgba is used, and then rviz will pay attention to it and render the transparency.

All the valid fields and types are here: http://wiki.ros.org/pcl/Overview The above could be added to point_cloud2.py, but you can see from the numerous fields that the number of convenience functions that anyone might want is very large. It would be useful to have a few more examples available.

edit flag offensive delete link more

Comments

Thanks !! that is exactly what I needed. The first message example is from a "depth_registered" topic from the openni driver so maybe there is more to it than just simple rgb.

Mehdi. gravatar imageMehdi. ( 2018-04-25 03:40:18 -0600 )edit

Does rviz show the colors in that depth_registered topic properly?

lucasw gravatar imagelucasw ( 2018-04-25 12:58:52 -0600 )edit

Your Answer

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

Add Answer

Question Tools

3 followers

Stats

Asked: 2018-04-24 13:15:16 -0600

Seen: 898 times

Last updated: Apr 24 '18