diff --git a/sensor_msgs_py/sensor_msgs_py/point_cloud2.py b/sensor_msgs_py/sensor_msgs_py/point_cloud2.py index 07f8ec3f..78fc734e 100644 --- a/sensor_msgs_py/sensor_msgs_py/point_cloud2.py +++ b/sensor_msgs_py/sensor_msgs_py/point_cloud2.py @@ -250,7 +250,9 @@ def create_cloud( for each point, with the elements of each iterable being the values of the fields for that point (in the same order as the fields parameter) - :param point_step: Point step size in bytes. Calculated from the given fields by default. + :param point_step: Point step size in bytes. If not provided, it is calculated + from the given fields, except when 'points' is a structured + NumPy array, in which case points.dtype.itemsize is used. (Type: optional of integer) :return: The point cloud as sensor_msgs.msg.PointCloud2 """ @@ -266,7 +268,9 @@ def create_cloud( points, dtype=dtype_from_fields(fields, point_step)) else: - assert points.dtype == dtype_from_fields(fields, point_step), \ + assert points.dtype == dtype_from_fields( + fields, + point_step or points.dtype.itemsize), \ 'PointFields and structured NumPy array dtype do not match for all fields! \ Check their field order, names and types.' else: diff --git a/sensor_msgs_py/test/test_point_cloud2.py b/sensor_msgs_py/test/test_point_cloud2.py index f63bf0ac..25e371bc 100644 --- a/sensor_msgs_py/test/test_point_cloud2.py +++ b/sensor_msgs_py/test/test_point_cloud2.py @@ -296,10 +296,18 @@ def test_create_cloud(self): def test_create_cloud_itemsize(self): with self.assertRaises(AssertionError): + dtype_from_fields = point_cloud2.dtype_from_fields(fields) point_cloud2.create_cloud( + Header(frame_id='frame'), + fields, + points_end_padding, + dtype_from_fields.itemsize) + + thispcd = point_cloud2.create_cloud( Header(frame_id='frame'), fields, points_end_padding) + self.assertEqual(thispcd.point_step, itemsize_end_padding) thispcd = point_cloud2.create_cloud( Header(frame_id='frame'),