ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | Q&A answers.ros.org
Ask Your Question
3

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

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

etch gravatar image

updated 2022-02-02 22:09:35 -0500

130s 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

Comments

Not an answer, but maybe some examples of converting ROS PointCloud2 from/to numpy arrays etc. there could help a bit

ljaniec gravatar image ljaniec  ( 2022-02-03 12:01:35 -0500 )edit

1 Answer

Sort by ยป oldest newest most voted
1

answered 2022-03-11 05:21:11 -0500

aburzio gravatar image

I used a modified version of the create_cloud_xyz32 function from point_cloud2.py

def create_cloud_xyzi32(header, points):

    fields = [PointField(name='x', offset=0,
                         datatype=PointField.FLOAT32, count=1),
              PointField(name='y', offset=4,
                         datatype=PointField.FLOAT32, count=1),
              PointField(name='z', offset=8,
                         datatype=PointField.FLOAT32, count=1),
              PointField(name='intensity', offset=12,
                         datatype=PointField.FLOAT32, count=1)]
    return create_cloud(header, fields, points)

This works for me with points being a list of tuples (x,y,z,intensity), and coverts it directly to a ROS2 message you can publish

Hope this helps :)

edit flag offensive delete link more

Your Answer

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

Add Answer

Question Tools

2 followers

Stats

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

Seen: 1,190 times

Last updated: Feb 02