Suggestion of sort the offesets in advance in fields_to_type() #40
Description
I encountered a ValueError in using point_cloud2.pointcloud2_to_xyz_array(). It has been figured out due to the disorder of offest among the PointFields.
The origin message(sensor_msgs/PointCloud2) has 'field' attriubte as below:
name: "x"
offset: 0
datatype: 7
count: 1, name: "y"
offset: 4
datatype: 7
count: 1, name: "z"
offset: 8
datatype: 7
count: 1, name: "intensity"
offset: 32
datatype: 7
count: 1, name: "normal_x"
offset: 16
datatype: 7
count: 1, name: "normal_y"
offset: 20
datatype: 7
count: 1, name: "normal_z"
offset: 24
datatype: 7
count: 1, name: "curvature"
offset: 36
datatype: 7
count: 1
Note that attribute "intensity" with offset 32 is arranged at the front of other attributes with larger offsets. As a result, in pointcloud2_to_array() in point_cloud2.py:
# construct a numpy record type equivalent to the point type of this cloud
dtype_list = fields_to_dtype(cloud_msg.fields, cloud_msg.point_step)
# parse the cloud into an array
cloud_arr = np.frombuffer(cloud_msg.data, dtype_list)I got dtype_list in such a strange form:
[('x', dtype('float32')), ('y', dtype('float32')), ('z', dtype('float32')), ('__12', <class 'numpy.uint8'>), ('__13', <class 'numpy.uint8'>), ('__14', <class 'numpy.uint8'>), ('__15', <class 'numpy.uint8'>), ('__16', <class 'numpy.uint8'>), ('__17', <class 'numpy.uint8'>), ('__18', <class 'numpy.uint8'>), ('__19', <class 'numpy.uint8'>), ('__20', <class 'numpy.uint8'>), ('__21', <class 'numpy.uint8'>), ('__22', <class 'numpy.uint8'>), ('__23', <class 'numpy.uint8'>), ('__24', <class 'numpy.uint8'>), ('__25', <class 'numpy.uint8'>), ('__26', <class 'numpy.uint8'>), ('__27', <class 'numpy.uint8'>), ('__28', <class 'numpy.uint8'>), ('__29', <class 'numpy.uint8'>), ('__30', <class 'numpy.uint8'>), ('__31', <class 'numpy.uint8'>), ('intensity', dtype('float32')), ('normal_x', dtype('float32')), ('normal_y', dtype('float32')), ('normal_z', dtype('float32')), ('curvature', dtype('float32'))]
Then it leads to mismatch in data structure and memory space and Error occurs in np.frombuffer()
Traceback (most recent call last):
File "/opt/ros/noetic/lib/python3/dist-packages/rospy/topics.py", line 750, in _invoke_callback
cb(msg)
File "/home/carto/lio_ws/src/faster-lio/src/map_builder.py", line 31, in pc_callback
self.point_cloud = ros_numpy.point_cloud2.pointcloud2_to_xyz_array(msg)
File "/home/carto/lio_ws/src/ros_numpy/src/ros_numpy/point_cloud2.py", line 244, in pointcloud2_to_xyz_array
return get_xyz_points(pointcloud2_to_array(cloud_msg), remove_nans=remove_nans)
File "/home/carto/lio_ws/src/ros_numpy/src/ros_numpy/point_cloud2.py", line 123, in pointcloud2_to_array
cloud_arr = np.frombuffer(cloud_msg.data, dtype_list)
ValueError: buffer size must be a multiple of element size
So I wonder whether we can sort the fields in advance in function fields_to_dtype or any other fix to avoid bugs dealing with msg with disorder field.