ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange
Ask Your Question

Revision history [back]

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, the sensor_msgs.point_cloud2 will help out a little though the packing of the rgb bytes :

#!/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

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, the sensor_msgs.point_cloud2 create_cloud will help out a little though (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/Overviewhttp://wiki.ros.org/pcl/Overview The above could be added to point_cloud2, but you can see from the numerous fields that the number of covenience functions that anyone might want is very large.

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, point_cloud2.py, but you can see from the numerous fields that the number of covenience convenience functions that anyone might want is very large.large. It would be useful to have a few more examples available.