Backport #199 to Humble

sensor_msgs_py/sensor_msgs_py/point_cloud2.py

@@ -91,7 +91,7 @@ def read_points(
     # Cast bytes to numpy array
     points = np.ndarray(
         shape=(cloud.width * cloud.height, ),
-        dtype=dtype_from_fields(cloud.fields),
+        dtype=dtype_from_fields(cloud.fields, point_step=cloud.point_step),
         buffer=cloud.data)
 
     # Keep only the requested fields
@@ -194,12 +194,14 @@ def read_points_list(
                                                 skip_nans, uvs)]
 
 
-def dtype_from_fields(fields: Iterable[PointField]) -> np.dtype:
+def dtype_from_fields(fields: Iterable[PointField], point_step: Optional[int] = None) -> np.dtype:
     """
     Convert a Iterable of sensor_msgs.msg.PointField messages to a np.dtype.
 
     :param fields: The point cloud fields.
                    (Type: iterable of sensor_msgs.msg.PointField)
+    :param point_step: Point step size in bytes. Calculated from the given fields by default.
+                       (Type: optional of integer)
     :returns: NumPy datatype
     """
     # Create a lists containing the names, offsets and datatypes of all fields
@@ -229,12 +231,15 @@ def dtype_from_fields(fields: Iterable[PointField]) -> np.dtype:
             field_offsets.append(field.offset + a * datatype.itemsize)
             field_datatypes.append(datatype.str)
 
-    # Create a tuple for each field containing name and data type
-    return np.dtype({
-        'names': field_names,
-        'formats': field_datatypes,
-        'offsets': field_offsets,
-    })
+    # Create dtype
+    dtype_dict = {
+            'names': field_names,
+            'formats': field_datatypes,
+            'offsets': field_offsets
+    }
+    if point_step is not None:
+        dtype_dict['itemsize'] = point_step
+    return np.dtype(dtype_dict)
 
 
 def create_cloud(

sensor_msgs_py/test/test_point_cloud2.py

@@ -32,9 +32,10 @@
 
 import numpy as np
 try:
-    from numpy.lib.recfunctions import structured_to_unstructured
+    from numpy.lib.recfunctions import structured_to_unstructured, unstructured_to_structured
 except ImportError:
-    from sensor_msgs_py.numpy_compat import structured_to_unstructured
+    from sensor_msgs_py.numpy_compat import (structured_to_unstructured,
+                                             unstructured_to_structured)
 
 from sensor_msgs.msg import PointCloud2, PointField
 from sensor_msgs_py import point_cloud2
@@ -270,6 +271,37 @@ def test_create_cloud__non_one_count(self):
             points)
         self.assertEqual(thispcd, pcd5)
 
+    def test_read_cloud_with_non_standard_point_step(self):
+        itemsize = 123  # Larger than normal point step size
+
+        # Copy to new array with larger itemsize
+        points_larger_itemsize = np.array(
+            unstructured_to_structured(points),
+            dtype=np.dtype({
+                'names': ['x', 'y', 'z'],
+                'formats': ['<f4', '<f4', '<f4'],
+                'offsets': [0, 4, 8],
+                'itemsize': itemsize
+            })
+        )
+
+        # Create pointcloud with itemsize == point_step from the padded array
+        pc = PointCloud2(
+            header=Header(frame_id='frame'),
+            height=1,
+            width=points_larger_itemsize.shape[0],
+            is_dense=False,
+            is_bigendian=sys.byteorder != 'little',
+            fields=fields,
+            point_step=itemsize,
+            row_step=itemsize * points_larger_itemsize.shape[0],
+            data=points_larger_itemsize.tobytes()
+        )
+
+        # Deserialize point cloud
+        reconstructed_points = point_cloud2.read_points_numpy(pc)
+        self.assertTrue(np.allclose(points, reconstructed_points, equal_nan=True))
+
 
 if __name__ == '__main__':
     unittest.main()