From 2a7e7a71745c57eb108a0d739579b2594e9f6010 Mon Sep 17 00:00:00 2001 From: xndcn Date: Tue, 9 Sep 2025 15:16:09 +0800 Subject: [PATCH] Use structured NumPy points.dtype.itemsize as default point_step in create_cloud Signed-off-by: xndcn --- sensor_msgs_py/sensor_msgs_py/point_cloud2.py | 8 ++++++-- sensor_msgs_py/test/test_point_cloud2.py | 8 ++++++++ 2 files changed, 14 insertions(+), 2 deletions(-) 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'),