Merge pull request #3 from UrbanMachine/rolling

Improve speed of `msgify(point_cloud_2_np)` by 115x

Author: tpanzarella

ros2_numpy/point_cloud2.py

@@ -39,6 +39,7 @@
 
 from .registry import converts_from_numpy, converts_to_numpy
 
+import array
 import numpy as np
 from sensor_msgs.msg import PointCloud2, PointField
 
@@ -165,7 +166,21 @@ def array_to_pointcloud2(cloud_arr, stamp=None, frame_id=None):
     cloud_msg.is_dense = \
       all([np.isfinite(
             cloud_arr[fname]).all() for fname in cloud_arr.dtype.names])
-    cloud_msg.data = cloud_arr.tostring()
+
+    # The PointCloud2.data setter will create an array.array object for you if you don't
+    # provide it one directly. This causes very slow performance because it iterates
+    # over each byte in python.
+    # Here we create an array.array object using a memoryview, limiting copying and
+    # increasing performance.
+    memory_view = memoryview(cloud_arr)
+    if memory_view.nbytes > 0:
+        array_bytes = memory_view.cast("B")
+    else:
+        # Casting raises a TypeError if the array has no elements
+        array_bytes = b""
+    as_array = array.array("B")
+    as_array.frombytes(array_bytes)
+    cloud_msg.data = as_array
     return cloud_msg
 
 def merge_rgb_fields(cloud_arr):

test/test_pointclouds.py

@@ -75,12 +75,19 @@ def test_roundtrip(self):
         np.testing.assert_equal(points_arr, new_points_arr)
 
     def test_roundtrip_numpy(self):
-
         points_arr = self.makeArray(100)
         cloud_msg = rnp.msgify(PointCloud2, points_arr)
         new_points_arr = rnp.numpify(cloud_msg)
 
         np.testing.assert_equal(points_arr, new_points_arr)
 
+    def test_roundtrip_zero_points(self):
+        """Test to make sure zero point arrays don't raise memoryview.cast(*) errors"""
+        points_arr = self.makeArray(0)
+        cloud_msg = rnp.msgify(PointCloud2, points_arr)
+        new_points_arr = rnp.numpify(cloud_msg)
+
+        np.testing.assert_equal(points_arr, new_points_arr)
+
 if __name__ == '__main__':
     unittest.main()