How to convert PCL(xyzi) to ROS PointCloud2 in python?

asked 2020-12-16 03:42:34 -0500

etch gravatar image

I'm using ROS Noetic on Ubuntu 20. I'm trying to work with point clouds using the python-pcl package.

I have a node that listens to a PointCloud2 type messages and converts it successfully to python-pcl. Now I want to convert the PCL_xyzi back to PointCloud2 so I can publish it on a topic. Can't figure out a way.

What I Tried:

def my_pcl_to_ros(cloud_pcl):
""" Converts a pcl to a ROS PointCloud2 message """

# Instantiating a PointCloud2 object.
cloud_ros = PointCloud2()
# Converting pcl to numpy array.
cloud = cloud_pcl.to_array()

# Filling the PointCLoud2 message definition #

#header
cloud_ros.header.stamp = rospy.Time.now()
cloud_ros.header.frame_id = "velodyne"

#width & height
h, w = cloud.shape
cloud_ros.width = w
cloud_ros.height = h

#fields
cloud_ros.fields.append(PointField(
                        name="x",
                        offset=0,
                        datatype=PointField.FLOAT32, count=1))
cloud_ros.fields.append(PointField(
                        name="y",
                        offset=4,
                        datatype=PointField.FLOAT32, count=1))
cloud_ros.fields.append(PointField(
                        name="z",
                        offset=8,
                        datatype=PointField.FLOAT32, count=1))
cloud_ros.fields.append(PointField(
                        name="intensity",
                        offset=12,
                        datatype=PointField.FLOAT32, count=1))

#booleans
cloud_ros.is_dense = False
cloud_ros.is_bigendian = False


#steps
# ps = getsizeof(cloud[0,0])
cloud_ros.point_step = 16
cloud_ros.row_step = ps * w

#data
#tostring converts to bytes, name is just misleading.
cloud_ros.data = cloud.tostring()

return cloud_ros

This converts it incorrectly, as when I try to convert this back to pcl, it gives me an error:

struct.error: unpack_from requires a buffer of at least 161104 bytes for unpacking 16 bytes at offset 161088 (actual buffer size is 161088)

I'm sure that my ros_to_pcl function is fine because the firstly recieved data is converted successfully.

The issue with the code above is probably the offset in the intensity PointField and could be the pointstep

I wrote the code comparing it to one that converts pcl_xyzrgb to PointCloud2, this a snippet of the code in which I believe I implemented falsely, ros_msg variable here is my cloud_ros variable above.

ros_msg.fields.append(PointField(
                        name="x",
                        offset=0,
                        datatype=PointField.FLOAT32, count=1))
ros_msg.fields.append(PointField(
                        name="y",
                        offset=4,
                        datatype=PointField.FLOAT32, count=1))
ros_msg.fields.append(PointField(
                        name="z",
                        offset=8,
                        datatype=PointField.FLOAT32, count=1))
ros_msg.fields.append(PointField(
                        name="rgb",
                        offset=16,
                        datatype=PointField.FLOAT32, count=1))

ros_msg.is_bigendian = False
ros_msg.point_step = 32
ros_msg.row_step = ros_msg.point_step * ros_msg.width * ros_msg.height
ros_msg.is_dense = False
buffer = []

for data in pcl_array:
    s = struct.pack('>f', data[3])

    i = struct.unpack('>l', s)[0]

    pack = ctypes.c_uint32(i).value

    r = (pack & 0x00FF0000) >> 16
    g = (pack & 0x0000FF00) >> 8
    b = (pack & 0x000000FF)

    buffer.append(struct.pack('ffffBBBBIII', data[0], data[1], data[2], 1.0, b, g, r, 0, 0, 0, 0))

ros_msg.data = b"".join(buffer)
edit retag flag offensive close merge delete