[sensor_msgs_py] Missing handling of point_step value in PointCloud2 message in read_points() fcn

[xgalma00]

Bug report

In sensor_msgs_py/sensor_msgs_py/point_cloud2.py is missing handling of point_step value from PointCloud2 message when reading the PC2 points (read_points()). Numpy dtype is automatically generated based on PointFields including the itemsize value. The end of the point (point_step) is expected just after the last item in the array. When the point_step is actually greater, the dtype is incorrectly aligned with the data array.

• Operating System:
  • Ubuntu 20.04
• Version or commit hash:
  • ROS2 Humble
• Package
  • sensor_msgs_py v4.2 or later

Example:

PointCloud2 message:

>>>print(pc_msg)
sensor_msgs.msg.PointCloud2(header=std_msgs.msg.Header(
stamp=builtin_interfaces.msg.Time(sec=1658393524, nanosec=629456477),
frame_id='tst_frame'), 
height=1, width=65, fields=[
sensor_msgs.msg.PointField(name='x', offset=0, datatype=7, count=1),
sensor_msgs.msg.PointField(name='y', offset=4, datatype=7, count=1),
sensor_msgs.msg.PointField(name='z', offset=8, datatype=7, count=1),
sensor_msgs.msg.PointField(name='normal_x', offset=16, datatype=7, count=1),
sensor_msgs.msg.PointField(name='normal_y', offset=20, datatype=7, count=1),
sensor_msgs.msg.PointField(name='normal_z', offset=24, datatype=7, count=1),
sensor_msgs.msg.PointField(name='curvature', offset=32, datatype=7, count=1)],
is_bigendian=False, point_step=48, row_step=3120, data=[124, ...], is_dense=False)

Resulting dtype:

>>>print(points.dtype)
{'names':['x','y','z','normal_x','normal_y','normal_z','curvature'],
'formats':['<f4','<f4','<f4','<f4','<f4','<f4','<f4'], 'offsets':[0,4,8,16,20,24,32],
'itemsize':36}

Correct dtype:

>>>print(points.dtype)
{'names':['x','y','z','normal_x','normal_y','normal_z','curvature'],
'formats':['<f4','<f4','<f4','<f4','<f4','<f4','<f4'], 'offsets':[0,4,8,16,20,24,32],
'itemsize':48}

[mrkbac]

Hey, is it possible to backport this fix to humble?

[gbiggs]

#211