Port pointcloud creation to numpy.

Author: Flova

sensor_msgs_py/package.xml

@@ -15,6 +15,7 @@
   <author email="sebastian.grans@gmail.com">Sebastian Grans</author>
 
   <exec_depend>sensor_msgs</exec_depend>
+  <exec_depend>python3-numpy</exec_depend>
 
   <test_depend>ament_copyright</test_depend>
   <test_depend>ament_flake8</test_depend>

sensor_msgs_py/sensor_msgs_py/point_cloud2.py

@@ -42,6 +42,7 @@
 import math
 import struct
 import sys
+import numpy as np
 
 from sensor_msgs.msg import PointCloud2, PointField
 
@@ -144,40 +145,6 @@ def read_points_list(cloud, field_names=None, skip_nans=False, uvs=[]):
                                                 skip_nans, uvs)]
 
 
-def create_cloud(header, fields, points):
-    """
-    Create a sensor_msgs.msg.PointCloud2 message.
-
-    :param header: The point cloud header. (Type: std_msgs.msg.Header)
-    :param fields: The point cloud fields.
-                   (Type: iterable of sensor_msgs.msg.PointField)
-    :param points: The point cloud points. List of iterables, i.e. one iterable
-                   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)
-    :return: The point cloud as sensor_msgs.msg.PointCloud2
-    """
-    cloud_struct = struct.Struct(_get_struct_fmt(False, fields))
-
-    buff = ctypes.create_string_buffer(cloud_struct.size * len(points))
-
-    point_step, pack_into = cloud_struct.size, cloud_struct.pack_into
-    offset = 0
-    for p in points:
-        pack_into(buff, offset, *p)
-        offset += point_step
-
-    return PointCloud2(header=header,
-                       height=1,
-                       width=len(points),
-                       is_dense=False,
-                       is_bigendian=False,
-                       fields=fields,
-                       point_step=cloud_struct.size,
-                       row_step=cloud_struct.size * len(points),
-                       data=buff.raw)
-
-
 def create_cloud_xyz32(header, points):
     """
     Create a sensor_msgs.msg.PointCloud2 message with (x, y, z) fields.
@@ -186,13 +153,39 @@ def create_cloud_xyz32(header, points):
     :param points: The point cloud points. (Type: Iterable)
     :return: The point cloud as sensor_msgs.msg.PointCloud2.
     """
-    fields = [PointField(name='x', offset=0,
-                         datatype=PointField.FLOAT32, count=1),
-              PointField(name='y', offset=4,
-                         datatype=PointField.FLOAT32, count=1),
-              PointField(name='z', offset=8,
-                         datatype=PointField.FLOAT32, count=1)]
-    return create_cloud(header, fields, points)
+    # Convert to numpy if it isn't already
+    points = np.asarray(points)
+    # Convert data to f32, flatten it and cat it to a byte string
+    data = points.astype(np.float32).reshape(-1).tobytes()
+    # Define offsets
+    point_value_bits = 32 # float32
+    point_num_values = 3 # 3 values are a point
+    point_value_bytes = point_value_bits // 8 # Number of bytes used for one value
+    point_bytes = (point_num_values * point_value_bits) // 8 # Bytes used by one point
+
+    # This function generates a point field describing the stored raw data for one dimension
+    def make_point_field(element):
+        i = element[0]
+        name = element[1]
+        return PointField(
+            name = name,
+            offset = i * point_value_bytes,
+            datatype = PointField.FLOAT32,
+            count = 1)
+
+    # Make a point field for each of the major axis
+    fields = list(map(make_point_field, enumerate(['x', 'y', 'z'])))
+    # Put everything together
+    return PointCloud2(
+        header = header,
+        height = 1,
+        width = len(points),
+        is_dense = False,
+        is_bigendian = False,
+        fields = fields,
+        point_step = point_bytes,
+        row_step = point_bytes * len(points),
+        data = data)
 
 
 def _get_struct_fmt(is_bigendian, fields, field_names=None):