Question

I am subscribing to a sensor_msgs/PointCloud2 message (points with: x,y,z,intensity) and I would like to change the intensity value based on some heuristic in my code.

In principle the code looks like this:

import sensor_msgs.point_cloud2

def handle_cloud(self, msg):
    field_names = ("x", "y", "z", "intensity")
    reader = sensor_msgs.point_cloud2.read_points(msg, field_names)
    for p in reader:
        new_intensity = some_code(p)
        # assign new intensity to point
        p[3] = new_intensity # TODO: is this possible somehow?
    # republish with new intensity values
    publisher.publish(msg)

Can I do this in-place, similarly to the code above? Or do I need to build a new message, using sensor_msgs.point_cloud2.create_cloud() ?

Asked by moooeeeep on 2019-06-04 09:22:25 UTC

Answer 1

This is possible, although the the PointCloud2 messages are not designed to be used this way. It may well take less code to create a new PointCloud2 message, however if you need to work on the message in-place for performance reasons then there is a way. Just to clarify if you're not doing this for performance reasons then I would recommend extracting the cloud from the message, modifying it and creating a new message.

The PointCloud2 message type stores the actual point data in it's data member as a byte array. it is possible to work out exactly where within this array each value lies and its type and then overwrite them in place.

The data structure of the PointCloud2 message is defined by an array of PointField Messages and the point_step and row_step byte spacing. If you're only going to work with a single type of point cloud you could ignore this an hard code the solution, but you'll need to look at it at least once to know what the structure is in the first place.

If you still want to try and do this and need some more pointers, let me know and I can give you some more detailed info.

Asked by PeteBlackerThe3rd on 2019-06-04 12:10:34 UTC

Comments

To be clear, this would involve adapting the code from here: http://docs.ros.org/melodic/api/sensor_msgs/html/point__cloud2_8py_source.html and do the struct unpacking and packing in my own code? I think I don't need it this badly. Thanks for the clarification!

Asked by moooeeeep on 2019-06-05 01:59:18 UTC

Yes that's what you'd need to do. Possible but far from easy! No problem

Asked by PeteBlackerThe3rd on 2019-06-05 06:29:23 UTC

In-Place Modification of PointCloud2 message?

Comments

Answers

Comments