From d6d652e838347cac3bf3bffdfa8299eba8a74bab Mon Sep 17 00:00:00 2001 From: Florian Vahl Date: Tue, 8 Mar 2022 11:20:34 +0100 Subject: [PATCH 01/20] Port pointcloud creation to numpy. Signed-off-by: Florian Vahl --- sensor_msgs_py/package.xml | 1 + sensor_msgs_py/sensor_msgs_py/point_cloud2.py | 198 +++++++++--------- 2 files changed, 95 insertions(+), 104 deletions(-) diff --git a/sensor_msgs_py/package.xml b/sensor_msgs_py/package.xml index 16e9adea..2cd57c87 100644 --- a/sensor_msgs_py/package.xml +++ b/sensor_msgs_py/package.xml @@ -15,6 +15,7 @@ Sebastian Grans sensor_msgs + python3-numpy ament_copyright ament_flake8 diff --git a/sensor_msgs_py/sensor_msgs_py/point_cloud2.py b/sensor_msgs_py/sensor_msgs_py/point_cloud2.py index 4a56c3cf..e0c0b1f5 100644 --- a/sensor_msgs_py/sensor_msgs_py/point_cloud2.py +++ b/sensor_msgs_py/sensor_msgs_py/point_cloud2.py @@ -37,27 +37,31 @@ sensor_msgs/src/sensor_msgs/point_cloud2.py """ -from collections import namedtuple -import ctypes -import math -import struct import sys +from collections import namedtuple +from typing import Iterable, List, Optional, NamedTuple +import numpy as np +import numpy.typing as npt from sensor_msgs.msg import PointCloud2, PointField - +from std_msgs.msg import Header _DATATYPES = {} -_DATATYPES[PointField.INT8] = ('b', 1) -_DATATYPES[PointField.UINT8] = ('B', 1) -_DATATYPES[PointField.INT16] = ('h', 2) -_DATATYPES[PointField.UINT16] = ('H', 2) -_DATATYPES[PointField.INT32] = ('i', 4) -_DATATYPES[PointField.UINT32] = ('I', 4) -_DATATYPES[PointField.FLOAT32] = ('f', 4) -_DATATYPES[PointField.FLOAT64] = ('d', 8) - - -def read_points(cloud, field_names=None, skip_nans=False, uvs=[]): +_DATATYPES[PointField.INT8] = (np.int8, 8) +_DATATYPES[PointField.UINT8] = (np.uint8, 8) +_DATATYPES[PointField.INT16] = (np.int16, 16) +_DATATYPES[PointField.UINT16] = (np.uint16, 16) +_DATATYPES[PointField.INT32] = (np.int32, 32) +_DATATYPES[PointField.UINT32] = (np.uint32, 32) +_DATATYPES[PointField.FLOAT32] = (np.float32, 32) +_DATATYPES[PointField.FLOAT64] = (np.float64, 64) + + +def read_points( + cloud: PointCloud2, + field_names: Optional[List[str]] = None, + skip_nans: bool = False, + uvs: Optional[Iterable] = None) -> npt.ArrayLike: """ Read points from a sensor_msgs.PointCloud2 message. @@ -67,56 +71,60 @@ def read_points(cloud, field_names=None, skip_nans=False, uvs=[]): :param skip_nans: If True, then don't return any point with a NaN value. (Type: Bool, Default: False) :param uvs: If specified, then only return the points at the given - coordinates. (Type: Iterable, Default: empty list) - :return: Generator which yields a list of values for each point. + coordinates. (Type: Iterable, Default: None) + :return: Numpy array containing all points. """ assert isinstance(cloud, PointCloud2), \ 'cloud is not a sensor_msgs.msg.PointCloud2' - fmt = _get_struct_fmt(cloud.is_bigendian, cloud.fields, field_names) - width, height, point_step, row_step, data, isnan = \ - cloud.width, cloud.height, \ - cloud.point_step, cloud.row_step, \ - cloud.data, math.isnan - unpack_from = struct.Struct(fmt).unpack_from + assert all(x.datatype==cloud.fields[0].datatype for x in cloud.fields), \ + 'cloud fields have non equal data types' + + # Create a list containing the indices of all used fields + if field_names is None: + field_ids = list(range(len(cloud.fields))) + else: + field_ids = list(map( + lambda field: field[0], + filter( + lambda field: field[1].name in field_names, + enumerate(cloud.fields)))) + + # Cast bytes to numpy array + points = np.ndarray( + shape=(cloud.width, len(cloud.fields)), + # Limitation: only one datatype per point cloud is possible, not per field + dtype=_DATATYPES[cloud.fields[0].datatype][0], + buffer=cloud.data) + + # Swap array if byte order does not match + if bool(sys.byteorder != "little") ^ bool(cloud.is_bigendian): + points = points.byteswap(inplace=True) if skip_nans: - if uvs: - for u, v in uvs: - p = unpack_from(data, (row_step * v) + (point_step * u)) - has_nan = False - for pv in p: - if isnan(pv): - has_nan = True - break - if not has_nan: - yield p - else: - for v in range(height): - offset = row_step * v - for u in range(width): - p = unpack_from(data, offset) - has_nan = False - for pv in p: - if isnan(pv): - has_nan = True - break - if not has_nan: - yield p - offset += point_step + # Mask points without any nan values + nan_mask = ~np.isnan(points).any(axis=1) + # Select these points + points = points[nan_mask] + + if uvs is None: + # Index requested fields + points = points[:, field_ids] else: - if uvs: - for u, v in uvs: - yield unpack_from(data, (row_step * v) + (point_step * u)) - else: - for v in range(height): - offset = row_step * v - for u in range(width): - yield unpack_from(data, offset) - offset += point_step - - -def read_points_list(cloud, field_names=None, skip_nans=False, uvs=[]): + # Don't convert to numpy if it is already one + if not isinstance(uvs, np.ndarray): + uvs = np.fromiter(uvs, int) + # Index requested points + points = points[uvs, field_ids] + + return points + + +def read_points_list( + cloud: PointCloud2, + field_names: Optional[List[str]] = None, + skip_nans: bool = False, + uvs: Optional[Iterable] = None) -> List[NamedTuple]: """ Read points from a sensor_msgs.PointCloud2 message. @@ -129,7 +137,7 @@ def read_points_list(cloud, field_names=None, skip_nans=False, uvs=[]): :param skip_nans: If True, then don't return any point with a NaN value. (Type: Bool, Default: False) :param uvs: If specified, then only return the points at the given - coordinates. (Type: Iterable, Default: empty list] + coordinates. (Type: Iterable, Default: None] :return: List of namedtuples containing the values for each point """ assert isinstance(cloud, PointCloud2), \ @@ -144,10 +152,12 @@ def read_points_list(cloud, field_names=None, skip_nans=False, uvs=[]): skip_nans, uvs)] -def create_cloud(header, fields, points): +def create_cloud( + header: Header, + fields: List[PointField], + points: Iterable) -> PointCloud2: """ 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) @@ -157,28 +167,28 @@ def create_cloud(header, fields, points): 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): + # Convert to numpy if it isn't already + points = np.asarray(points, dtype=_DATATYPES[fields[0].datatype][0]) + # Convert data to f32, flatten it and cat it to a byte string + data = points.reshape(-1).tobytes() + # Define offsets + point_value_bits = _DATATYPES[fields[0].datatype][1] + point_num_values = len(fields) + point_bytes = (point_num_values * point_value_bits) // 8 # Bytes used by one point + # 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 create_cloud_xyz32(header: Header, points: Iterable) -> PointCloud2: """ Create a sensor_msgs.msg.PointCloud2 message with (x, y, z) fields. @@ -193,23 +203,3 @@ def create_cloud_xyz32(header, points): PointField(name='z', offset=8, datatype=PointField.FLOAT32, count=1)] return create_cloud(header, fields, points) - - -def _get_struct_fmt(is_bigendian, fields, field_names=None): - fmt = '>' if is_bigendian else '<' - - offset = 0 - for field in (f for f in sorted(fields, key=lambda f: f.offset) - if field_names is None or f.name in field_names): - if offset < field.offset: - fmt += 'x' * (field.offset - offset) - offset = field.offset - if field.datatype not in _DATATYPES: - print('Skipping unknown PointField datatype [%d]' % - field.datatype, file=sys.stderr) - else: - datatype_fmt, datatype_length = _DATATYPES[field.datatype] - fmt += field.count * datatype_fmt - offset += field.count * datatype_length - - return fmt From 26dcbf7e7e957d5349dfd6bae654ac89e840a3d6 Mon Sep 17 00:00:00 2001 From: Florian Vahl Date: Tue, 8 Mar 2022 13:28:44 +0100 Subject: [PATCH 02/20] Add support for differently typed fields. Signed-off-by: Florian Vahl --- sensor_msgs_py/sensor_msgs_py/point_cloud2.py | 75 ++++++++++++------- 1 file changed, 46 insertions(+), 29 deletions(-) diff --git a/sensor_msgs_py/sensor_msgs_py/point_cloud2.py b/sensor_msgs_py/sensor_msgs_py/point_cloud2.py index e0c0b1f5..745b387f 100644 --- a/sensor_msgs_py/sensor_msgs_py/point_cloud2.py +++ b/sensor_msgs_py/sensor_msgs_py/point_cloud2.py @@ -61,7 +61,8 @@ def read_points( cloud: PointCloud2, field_names: Optional[List[str]] = None, skip_nans: bool = False, - uvs: Optional[Iterable] = None) -> npt.ArrayLike: + uvs: Optional[Iterable] = None, + reshape_organized_cloud: bool = False) -> npt.ArrayLike: """ Read points from a sensor_msgs.PointCloud2 message. @@ -75,47 +76,63 @@ def read_points( :return: Numpy array containing all points. """ assert isinstance(cloud, PointCloud2), \ - 'cloud is not a sensor_msgs.msg.PointCloud2' - - assert all(x.datatype==cloud.fields[0].datatype for x in cloud.fields), \ - 'cloud fields have non equal data types' - - # Create a list containing the indices of all used fields - if field_names is None: - field_ids = list(range(len(cloud.fields))) - else: - field_ids = list(map( - lambda field: field[0], - filter( - lambda field: field[1].name in field_names, - enumerate(cloud.fields)))) + 'Cloud is not a sensor_msgs.msg.PointCloud2' + + # Create a list containing the names of all fields + all_field_names = [] + for i, field in enumerate(cloud.fields): + if field.name == "": + name = f"unnamed_field_{i}" + else: + name = field.name + assert not name in all_field_names, "Duplicate field names are not allowed!" + all_field_names.append(name) + + # Create a tuple for each field containing name and data type + dtype = np.dtype({ + 'names': all_field_names, + 'formats': [np.dtype(_DATATYPES[field.datatype][0]).str for field in cloud.fields], + 'offsets': [field.offset for field in cloud.fields], + }) # Cast bytes to numpy array points = np.ndarray( - shape=(cloud.width, len(cloud.fields)), - # Limitation: only one datatype per point cloud is possible, not per field - dtype=_DATATYPES[cloud.fields[0].datatype][0], + shape=(cloud.width * cloud.height, ), + dtype=dtype, buffer=cloud.data) + # Keep the only requested fields + if field_names is not None: + assert all(field_name in all_field_names for field_name in field_names), \ + "Requests field is not in the fields of the PointCloud!" + # Mask fields + points = points[list(field_names)] + # Swap array if byte order does not match - if bool(sys.byteorder != "little") ^ bool(cloud.is_bigendian): + if bool(sys.byteorder != "little") != bool(cloud.is_bigendian): points = points.byteswap(inplace=True) - if skip_nans: - # Mask points without any nan values - nan_mask = ~np.isnan(points).any(axis=1) + # Check if we want to drop points with nan values + if skip_nans and not cloud.is_dense: + # Init mask which selects all points + not_nan_mask = np.ones(len(points), dtype=np.bool) + for field_name in points.dtype.names: + # Only keep points without any non values in the mask + not_nan_mask = np.logical_and(not_nan_mask, ~np.isnan(points[field_name])) # Select these points - points = points[nan_mask] + points = points[not_nan_mask] - if uvs is None: - # Index requested fields - points = points[:, field_ids] - else: - # Don't convert to numpy if it is already one + # Select points indexed by the uvs field + if uvs is not None: + # Don't convert to numpy array if it is already one if not isinstance(uvs, np.ndarray): uvs = np.fromiter(uvs, int) # Index requested points - points = points[uvs, field_ids] + points = points[uvs] + + # Cast into 2d array if cloud is 'organized' + if reshape_organized_cloud and cloud.height > 1: + points = points.reshape(cloud.height, cloud.width) return points From 0a0e4a52129069644f8fb9807ec4fe70688f5991 Mon Sep 17 00:00:00 2001 From: Florian Vahl Date: Tue, 8 Mar 2022 14:13:20 +0100 Subject: [PATCH 03/20] Add support for reading structured arrays Signed-off-by: Florian Vahl --- sensor_msgs_py/sensor_msgs_py/point_cloud2.py | 36 +++++++++++++++++-- 1 file changed, 33 insertions(+), 3 deletions(-) diff --git a/sensor_msgs_py/sensor_msgs_py/point_cloud2.py b/sensor_msgs_py/sensor_msgs_py/point_cloud2.py index 745b387f..4ee06718 100644 --- a/sensor_msgs_py/sensor_msgs_py/point_cloud2.py +++ b/sensor_msgs_py/sensor_msgs_py/point_cloud2.py @@ -42,7 +42,7 @@ from typing import Iterable, List, Optional, NamedTuple import numpy as np -import numpy.typing as npt +from numpy.lib.recfunctions import structured_to_unstructured from sensor_msgs.msg import PointCloud2, PointField from std_msgs.msg import Header @@ -62,7 +62,7 @@ def read_points( field_names: Optional[List[str]] = None, skip_nans: bool = False, uvs: Optional[Iterable] = None, - reshape_organized_cloud: bool = False) -> npt.ArrayLike: + reshape_organized_cloud: bool = False) -> np.ndarray: """ Read points from a sensor_msgs.PointCloud2 message. @@ -73,7 +73,7 @@ def read_points( (Type: Bool, Default: False) :param uvs: If specified, then only return the points at the given coordinates. (Type: Iterable, Default: None) - :return: Numpy array containing all points. + :return: Structured NumPy array containing all points. """ assert isinstance(cloud, PointCloud2), \ 'Cloud is not a sensor_msgs.msg.PointCloud2' @@ -137,6 +137,36 @@ def read_points( return points +def read_points_numpy( + cloud: PointCloud2, + field_names: Optional[List[str]] = None, + skip_nans: bool = False, + uvs: Optional[Iterable] = None, + reshape_organized_cloud: bool = False) -> np.ndarray: + """ + Read equally typed fields from sensor_msgs.PointCloud2 message + as a unstructured numpy array. + + This method is better suited if one wants to perform build math operations + on e.g. all x,y,z fields. + But it is limited to fields with the same dtype as unstructured numpy arrays + only contain one dtype. + + :param cloud: The point cloud to read from sensor_msgs.PointCloud2. + :param field_names: The names of fields to read. If None, read all fields. + (Type: Iterable, Default: None) + :param skip_nans: If True, then don't return any point with a NaN value. + (Type: Bool, Default: False) + :param uvs: If specified, then only return the points at the given + coordinates. (Type: Iterable, Default: None) + :return: Numpy array containing all points. + """ + assert all(cloud.fields[0].datatype == field.datatype for field in cloud.fields[1:]), \ + "All fields need to have the same datatype. Use `read_points()` otherwise." + structured_numpy_array = read_points(cloud, field_names, skip_nans, uvs, reshape_organized_cloud) + return structured_to_unstructured(structured_numpy_array) + + def read_points_list( cloud: PointCloud2, field_names: Optional[List[str]] = None, From e393390bad2cd43d4e750f19864c782902c1463f Mon Sep 17 00:00:00 2001 From: Florian Vahl Date: Tue, 8 Mar 2022 17:17:49 +0100 Subject: [PATCH 04/20] Adapt point cloud creation to utilize organized point clouds and multiple field datatypes Signed-off-by: Florian Vahl --- sensor_msgs_py/sensor_msgs_py/point_cloud2.py | 126 ++++++++++++------ 1 file changed, 82 insertions(+), 44 deletions(-) diff --git a/sensor_msgs_py/sensor_msgs_py/point_cloud2.py b/sensor_msgs_py/sensor_msgs_py/point_cloud2.py index 4ee06718..7b7a3f10 100644 --- a/sensor_msgs_py/sensor_msgs_py/point_cloud2.py +++ b/sensor_msgs_py/sensor_msgs_py/point_cloud2.py @@ -42,19 +42,20 @@ from typing import Iterable, List, Optional, NamedTuple import numpy as np -from numpy.lib.recfunctions import structured_to_unstructured +from numpy.lib.recfunctions import \ + structured_to_unstructured, unstructured_to_structured from sensor_msgs.msg import PointCloud2, PointField from std_msgs.msg import Header _DATATYPES = {} -_DATATYPES[PointField.INT8] = (np.int8, 8) -_DATATYPES[PointField.UINT8] = (np.uint8, 8) -_DATATYPES[PointField.INT16] = (np.int16, 16) -_DATATYPES[PointField.UINT16] = (np.uint16, 16) -_DATATYPES[PointField.INT32] = (np.int32, 32) -_DATATYPES[PointField.UINT32] = (np.uint32, 32) -_DATATYPES[PointField.FLOAT32] = (np.float32, 32) -_DATATYPES[PointField.FLOAT64] = (np.float64, 64) +_DATATYPES[PointField.INT8] = np.dtype(np.int8) +_DATATYPES[PointField.UINT8] = np.dtype(np.uint8) +_DATATYPES[PointField.INT16] = np.dtype(np.int16) +_DATATYPES[PointField.UINT16] = np.dtype(np.uint16) +_DATATYPES[PointField.INT32] = np.dtype(np.int32) +_DATATYPES[PointField.UINT32] = np.dtype(np.uint32) +_DATATYPES[PointField.FLOAT32] = np.dtype(np.float32) +_DATATYPES[PointField.FLOAT64] = np.dtype(np.float64) def read_points( @@ -78,32 +79,15 @@ def read_points( assert isinstance(cloud, PointCloud2), \ 'Cloud is not a sensor_msgs.msg.PointCloud2' - # Create a list containing the names of all fields - all_field_names = [] - for i, field in enumerate(cloud.fields): - if field.name == "": - name = f"unnamed_field_{i}" - else: - name = field.name - assert not name in all_field_names, "Duplicate field names are not allowed!" - all_field_names.append(name) - - # Create a tuple for each field containing name and data type - dtype = np.dtype({ - 'names': all_field_names, - 'formats': [np.dtype(_DATATYPES[field.datatype][0]).str for field in cloud.fields], - 'offsets': [field.offset for field in cloud.fields], - }) - # Cast bytes to numpy array points = np.ndarray( shape=(cloud.width * cloud.height, ), - dtype=dtype, + dtype=dtype_from_fields(cloud.fields), buffer=cloud.data) # Keep the only requested fields if field_names is not None: - assert all(field_name in all_field_names for field_name in field_names), \ + assert all(field_name in points.dtype.names for field_name in field_names), \ "Requests field is not in the fields of the PointCloud!" # Mask fields points = points[list(field_names)] @@ -132,7 +116,7 @@ def read_points( # Cast into 2d array if cloud is 'organized' if reshape_organized_cloud and cloud.height > 1: - points = points.reshape(cloud.height, cloud.width) + points = points.reshape(cloud.width, cloud.height) return points @@ -199,9 +183,35 @@ def read_points_list( skip_nans, uvs)] +def dtype_from_fields(fields: Iterable[PointField]) -> np.dtype: + """ + Converts 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) + :returns: NumPy datatype + """ + # Create a list containing the names of all fields + field_names = [] + for i, field in enumerate(fields): + if field.name == "": + name = f"unnamed_field_{i}" + else: + name = field.name + assert not name in field_names, "Duplicate field names are not allowed!" + field_names.append(name) + + # Create a tuple for each field containing name and data type + return np.dtype({ + 'names': field_names, + 'formats': [_DATATYPES[field.datatype].str for field in fields], + 'offsets': [field.offset for field in fields], + }) + + def create_cloud( header: Header, - fields: List[PointField], + fields: Iterable[PointField], points: Iterable) -> PointCloud2: """ Create a sensor_msgs.msg.PointCloud2 message. @@ -214,25 +224,53 @@ def create_cloud( the fields parameter) :return: The point cloud as sensor_msgs.msg.PointCloud2 """ - # Convert to numpy if it isn't already - points = np.asarray(points, dtype=_DATATYPES[fields[0].datatype][0]) - # Convert data to f32, flatten it and cat it to a byte string - data = points.reshape(-1).tobytes() - # Define offsets - point_value_bits = _DATATYPES[fields[0].datatype][1] - point_num_values = len(fields) - point_bytes = (point_num_values * point_value_bits) // 8 # Bytes used by one point + # Check if input is numpy array + if isinstance(points, np.ndarray): + # Check if this is an unstructured array + if points.dtype.names is None: + assert all(fields[0].datatype == field.datatype for field in fields[1:]), \ + "All fields need to have the same datatype. Pass a structured NumPy array with multiple dtypes otherwise." + # Convert unstructured to structured array + points = unstructured_to_structured( + points, + dtype = dtype_from_fields(fields)) + else: + assert len(points.dtype.names) == len(fields), \ + "The number of fields in the structured NumPy array and the PointFields do not match!" + assert all(points.dtype.formats[i] == _DATATYPES[fields[i].datatype].str for i in range(len(fields))), \ + f"PointField and structured NumPy array do not match data types for all fields! \ + Check their order. \n \ + The field datatypes are: {','.join(list(map(lambda field: _DATATYPES[field.datatype].str, fields)))} \n \ + The NumPy datatypes are: {','.join(points.dtype.formats)}" + # TODO check if that is the proper way to rename this + points.dtype.names = [field.name for field in fields] + else: + # Cast python objects to structured NumPy array (slow) + points = np.array( + # Points need to be tuples in the structured array + list(map(tuple, points)), + dtype=dtype_from_fields(fields)) + + # Handle organized clouds + assert len(points.shape) <= 2, \ + "Too many dimensions for organized cloud! Points can only be organized in max. two dimensional space" + height = 1 + width = points.shape[0] + # Check if input points where an organized cloud (2D array of points) + if len(points.shape) == 2: + height = points.shape[1] + # Put everything together return PointCloud2( header = header, - height = 1, - width = len(points), + height = height, + width = width, is_dense = False, - is_bigendian = False, + is_bigendian = sys.byteorder != 'little', fields = fields, - point_step = point_bytes, - row_step = point_bytes * len(points), - data = data) + point_step = points.dtype.itemsize, + row_step = (points.dtype.itemsize * len(points)) // height, + data = points.tobytes()) def create_cloud_xyz32(header: Header, points: Iterable) -> PointCloud2: From c5ba2c2888c82c6214c2428b4b873fb2d711f060 Mon Sep 17 00:00:00 2001 From: Florian Vahl Date: Tue, 8 Mar 2022 17:18:47 +0100 Subject: [PATCH 05/20] Format imports Signed-off-by: Florian Vahl --- sensor_msgs_py/sensor_msgs_py/point_cloud2.py | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/sensor_msgs_py/sensor_msgs_py/point_cloud2.py b/sensor_msgs_py/sensor_msgs_py/point_cloud2.py index 7b7a3f10..007bed33 100644 --- a/sensor_msgs_py/sensor_msgs_py/point_cloud2.py +++ b/sensor_msgs_py/sensor_msgs_py/point_cloud2.py @@ -39,11 +39,11 @@ import sys from collections import namedtuple -from typing import Iterable, List, Optional, NamedTuple +from typing import Iterable, List, NamedTuple, Optional import numpy as np -from numpy.lib.recfunctions import \ - structured_to_unstructured, unstructured_to_structured +from numpy.lib.recfunctions import (structured_to_unstructured, + unstructured_to_structured) from sensor_msgs.msg import PointCloud2, PointField from std_msgs.msg import Header From a715a5cd243fbdde84bcefb9c09c390000dc9e79 Mon Sep 17 00:00:00 2001 From: Florian Vahl Date: Wed, 9 Mar 2022 19:24:36 +0100 Subject: [PATCH 06/20] Apply autopep8 Signed-off-by: Florian Vahl --- sensor_msgs_py/sensor_msgs_py/point_cloud2.py | 26 ++++++++++--------- 1 file changed, 14 insertions(+), 12 deletions(-) diff --git a/sensor_msgs_py/sensor_msgs_py/point_cloud2.py b/sensor_msgs_py/sensor_msgs_py/point_cloud2.py index 007bed33..0e76c840 100644 --- a/sensor_msgs_py/sensor_msgs_py/point_cloud2.py +++ b/sensor_msgs_py/sensor_msgs_py/point_cloud2.py @@ -102,7 +102,8 @@ def read_points( not_nan_mask = np.ones(len(points), dtype=np.bool) for field_name in points.dtype.names: # Only keep points without any non values in the mask - not_nan_mask = np.logical_and(not_nan_mask, ~np.isnan(points[field_name])) + not_nan_mask = np.logical_and( + not_nan_mask, ~np.isnan(points[field_name])) # Select these points points = points[not_nan_mask] @@ -147,7 +148,8 @@ def read_points_numpy( """ assert all(cloud.fields[0].datatype == field.datatype for field in cloud.fields[1:]), \ "All fields need to have the same datatype. Use `read_points()` otherwise." - structured_numpy_array = read_points(cloud, field_names, skip_nans, uvs, reshape_organized_cloud) + structured_numpy_array = read_points( + cloud, field_names, skip_nans, uvs, reshape_organized_cloud) return structured_to_unstructured(structured_numpy_array) @@ -233,7 +235,7 @@ def create_cloud( # Convert unstructured to structured array points = unstructured_to_structured( points, - dtype = dtype_from_fields(fields)) + dtype=dtype_from_fields(fields)) else: assert len(points.dtype.names) == len(fields), \ "The number of fields in the structured NumPy array and the PointFields do not match!" @@ -262,15 +264,15 @@ def create_cloud( # Put everything together return PointCloud2( - header = header, - height = height, - width = width, - is_dense = False, - is_bigendian = sys.byteorder != 'little', - fields = fields, - point_step = points.dtype.itemsize, - row_step = (points.dtype.itemsize * len(points)) // height, - data = points.tobytes()) + header=header, + height=height, + width=width, + is_dense=False, + is_bigendian=sys.byteorder != 'little', + fields=fields, + point_step=points.dtype.itemsize, + row_step=(points.dtype.itemsize * len(points)) // height, + data=points.tobytes()) def create_cloud_xyz32(header: Header, points: Iterable) -> PointCloud2: From 2bcf11788616a31e7d88b6d560958ca798927c69 Mon Sep 17 00:00:00 2001 From: Florian Vahl Date: Wed, 9 Mar 2022 19:42:00 +0100 Subject: [PATCH 07/20] Add python tmp files to .gitignore Signed-off-by: Florian Vahl --- .gitignore | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/.gitignore b/.gitignore index b8bd0267..9cfa5df3 100644 --- a/.gitignore +++ b/.gitignore @@ -26,3 +26,7 @@ *.exe *.out *.app + +# Python temporary files +*.pyc +__pycache__/ From 6bdd743f8b889b651719470e4232d7d0c6a7b888 Mon Sep 17 00:00:00 2001 From: Florian Vahl Date: Wed, 9 Mar 2022 19:42:25 +0100 Subject: [PATCH 08/20] Fix docsting test Signed-off-by: Florian Vahl --- sensor_msgs_py/sensor_msgs_py/point_cloud2.py | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/sensor_msgs_py/sensor_msgs_py/point_cloud2.py b/sensor_msgs_py/sensor_msgs_py/point_cloud2.py index 0e76c840..d340ddb9 100644 --- a/sensor_msgs_py/sensor_msgs_py/point_cloud2.py +++ b/sensor_msgs_py/sensor_msgs_py/point_cloud2.py @@ -129,8 +129,7 @@ def read_points_numpy( uvs: Optional[Iterable] = None, reshape_organized_cloud: bool = False) -> np.ndarray: """ - Read equally typed fields from sensor_msgs.PointCloud2 message - as a unstructured numpy array. + Read equally typed fields from sensor_msgs.PointCloud2 message as a unstructured numpy array. This method is better suited if one wants to perform build math operations on e.g. all x,y,z fields. @@ -187,7 +186,7 @@ def read_points_list( def dtype_from_fields(fields: Iterable[PointField]) -> np.dtype: """ - Converts a Iterable of sensor_msgs.msg.PointField messages to a 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) @@ -217,6 +216,7 @@ def create_cloud( points: Iterable) -> PointCloud2: """ 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) From 7e311e1f10ecddcfd5855d8881f36e7f281b6254 Mon Sep 17 00:00:00 2001 From: Florian Vahl Date: Wed, 9 Mar 2022 20:05:48 +0100 Subject: [PATCH 09/20] Apply flake8 style Signed-off-by: Florian Vahl --- sensor_msgs_py/sensor_msgs_py/point_cloud2.py | 25 +++++++++++-------- 1 file changed, 15 insertions(+), 10 deletions(-) diff --git a/sensor_msgs_py/sensor_msgs_py/point_cloud2.py b/sensor_msgs_py/sensor_msgs_py/point_cloud2.py index d340ddb9..e0f4e05c 100644 --- a/sensor_msgs_py/sensor_msgs_py/point_cloud2.py +++ b/sensor_msgs_py/sensor_msgs_py/point_cloud2.py @@ -99,7 +99,7 @@ def read_points( # Check if we want to drop points with nan values if skip_nans and not cloud.is_dense: # Init mask which selects all points - not_nan_mask = np.ones(len(points), dtype=np.bool) + not_nan_mask = np.ones(len(points), dtype=bool) for field_name in points.dtype.names: # Only keep points without any non values in the mask not_nan_mask = np.logical_and( @@ -199,7 +199,7 @@ def dtype_from_fields(fields: Iterable[PointField]) -> np.dtype: name = f"unnamed_field_{i}" else: name = field.name - assert not name in field_names, "Duplicate field names are not allowed!" + assert name not in field_names, "Duplicate field names are not allowed!" field_names.append(name) # Create a tuple for each field containing name and data type @@ -231,19 +231,23 @@ def create_cloud( # Check if this is an unstructured array if points.dtype.names is None: assert all(fields[0].datatype == field.datatype for field in fields[1:]), \ - "All fields need to have the same datatype. Pass a structured NumPy array with multiple dtypes otherwise." + "All fields need to have the same datatype. Pass a structured NumPy array \ + with multiple dtypes otherwise." # Convert unstructured to structured array points = unstructured_to_structured( points, dtype=dtype_from_fields(fields)) else: assert len(points.dtype.names) == len(fields), \ - "The number of fields in the structured NumPy array and the PointFields do not match!" - assert all(points.dtype.formats[i] == _DATATYPES[fields[i].datatype].str for i in range(len(fields))), \ - f"PointField and structured NumPy array do not match data types for all fields! \ - Check their order. \n \ - The field datatypes are: {','.join(list(map(lambda field: _DATATYPES[field.datatype].str, fields)))} \n \ - The NumPy datatypes are: {','.join(points.dtype.formats)}" + "The number of fields in the structured NumPy \ + array and the PointFields do not match!" + + all_fields_have_matching_datatypes = all(map( + lambda i: points.dtype.formats[i] == _DATATYPES[fields[i].datatype].str, + range(len(fields)))) + assert all_fields_have_matching_datatypes, \ + "PointField and structured NumPy array do not match data types for all fields! \ + Check their order and types." # TODO check if that is the proper way to rename this points.dtype.names = [field.name for field in fields] else: @@ -255,7 +259,8 @@ def create_cloud( # Handle organized clouds assert len(points.shape) <= 2, \ - "Too many dimensions for organized cloud! Points can only be organized in max. two dimensional space" + "Too many dimensions for organized cloud! \ + Points can only be organized in max. two dimensional space" height = 1 width = points.shape[0] # Check if input points where an organized cloud (2D array of points) From e011bd38de86393d3cca1bc074c15773aa43fd80 Mon Sep 17 00:00:00 2001 From: Florian Vahl Date: Wed, 9 Mar 2022 20:07:41 +0100 Subject: [PATCH 10/20] Add list cast to points. \n They are tuples otherwise, which lets the check fail because list!=tuple Signed-off-by: Florian Vahl --- sensor_msgs_py/test/test_point_cloud2.py | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/sensor_msgs_py/test/test_point_cloud2.py b/sensor_msgs_py/test/test_point_cloud2.py index f2851b0d..8adf8801 100644 --- a/sensor_msgs_py/test/test_point_cloud2.py +++ b/sensor_msgs_py/test/test_point_cloud2.py @@ -90,12 +90,12 @@ class TestPointCloud2Methods(unittest.TestCase): def test_read_points(self): # Test that converting a PointCloud2 to a list, is equivalent to # the original list of points. - pcd_list = list(point_cloud2.read_points(pcd)) + pcd_list = list(map(list, point_cloud2.read_points(pcd))) self.assertTrue(np.allclose(pcd_list, pylist, equal_nan=True)) def test_read_points_field(self): # Test that field selection is working. - pcd_list = list(point_cloud2.read_points(pcd, field_names=['x', 'z'])) + pcd_list = list(map(list, point_cloud2.read_points(pcd, field_names=['x', 'z']))) # Check correct shape. self.assertTrue(np.array(pcd_list).shape == points[:, [0, 2]].shape) # Check "correct" values. @@ -104,7 +104,7 @@ def test_read_points_field(self): def test_read_points_skip_nan(self): # Test that removing NaNs work. - pcd_list = list(point_cloud2.read_points(pcd, skip_nans=True)) + pcd_list = list(map(list, point_cloud2.read_points(pcd, skip_nans=True))) points_nonan = points[~np.any(np.isnan(points), axis=1)] # Check correct shape self.assertTrue(np.array(pcd_list).shape == points_nonan.shape) From 91edf890723296f47721e1f2d54cff88fb03a1cc Mon Sep 17 00:00:00 2001 From: Florian Vahl Date: Wed, 9 Mar 2022 22:34:03 +0100 Subject: [PATCH 11/20] Add more tests Signed-off-by: Florian Vahl --- sensor_msgs_py/test/test_point_cloud2.py | 125 ++++++++++++++++++++--- 1 file changed, 110 insertions(+), 15 deletions(-) diff --git a/sensor_msgs_py/test/test_point_cloud2.py b/sensor_msgs_py/test/test_point_cloud2.py index 8adf8801..b0594c57 100644 --- a/sensor_msgs_py/test/test_point_cloud2.py +++ b/sensor_msgs_py/test/test_point_cloud2.py @@ -27,21 +27,21 @@ # POSSIBILITY OF SUCH DAMAGE. +import sys import unittest import numpy as np - -from sensor_msgs.msg import PointCloud2 -from sensor_msgs.msg import PointField +from numpy.lib.recfunctions import structured_to_unstructured +from sensor_msgs.msg import PointCloud2, PointField from sensor_msgs_py import point_cloud2 from std_msgs.msg import Header - pylist = [[0.0, 0.1, 0.2], [1.0, 1.1, 1.2], [2.0, 2.1, 2.2], [3.0, 3.1, 3.2], - [4.0, np.nan, 4.2]] + [4.0, np.nan, 4.2], + [5.0, 5.1, 5.2]] points = np.array(pylist, dtype=np.float32) fields = [PointField(name='x', offset=0, datatype=PointField.FLOAT32, count=1), @@ -57,7 +57,7 @@ height=1, width=points.shape[0], is_dense=False, - is_bigendian=False, # Not sure how to properly determine this. + is_bigendian=sys.byteorder != 'little', fields=fields, point_step=(itemsize * 3), # Every point consists of three float32s. row_step=(itemsize * 3 * points.shape[0]), @@ -77,13 +77,59 @@ height=1, width=points.shape[0], is_dense=False, - is_bigendian=False, # Not sure how to properly determine this. + is_bigendian=sys.byteorder != 'little', fields=fields, - point_step=(itemsize * 2), # Every point consists of three float32s. + point_step=(itemsize * 2), # Every point consists of two float32s. row_step=(itemsize * 2 * points.shape[0]), data=data ) +# Organized point cloud (Points are aligned in 2D matrix) +points3 = points.reshape(2, 3, -1) +pcd3 = PointCloud2( + header=Header(frame_id='frame'), + height=points3.shape[1], + width=points3.shape[0], + is_dense=False, + is_bigendian=sys.byteorder != 'little', + fields=fields, + # Every point consists of three float32s. + point_step=(itemsize * points3.shape[-1]), + row_step=(itemsize * points3.shape[-1] * points3.shape[0]), + data=points3.tobytes() +) + +# Check multiple datatype pointclouds +struct_points = np.array( + # Make each point a tuple + list(map(tuple, points)), + dtype=[ + ("a", np.float32), + ("b", np.float64), + ("c", np.uint8), + ]) + +struct_points_itemsize = struct_points.itemsize + +struct_points_fields = [ + PointField(name='a', offset=0, datatype=PointField.FLOAT32, count=1), + PointField(name='b', offset=4, datatype=PointField.FLOAT64, count=1), + PointField(name='c', offset=12, datatype=PointField.UINT8, count=1)] + +pcd4 = PointCloud2( + header=Header(frame_id='frame'), + height=1, + width=struct_points.shape[0], + is_dense=False, + is_bigendian=sys.byteorder != 'little', + fields=struct_points_fields, + # This time a struct array is used. + # The itemsize therfore represents the size of a complete point + point_step=struct_points_itemsize, + row_step=(struct_points_itemsize * points.shape[0]), + data=struct_points.tobytes() +) + class TestPointCloud2Methods(unittest.TestCase): @@ -95,7 +141,9 @@ def test_read_points(self): def test_read_points_field(self): # Test that field selection is working. - pcd_list = list(map(list, point_cloud2.read_points(pcd, field_names=['x', 'z']))) + pcd_list = list(map( + list, + point_cloud2.read_points(pcd, field_names=['x', 'z']))) # Check correct shape. self.assertTrue(np.array(pcd_list).shape == points[:, [0, 2]].shape) # Check "correct" values. @@ -104,7 +152,9 @@ def test_read_points_field(self): def test_read_points_skip_nan(self): # Test that removing NaNs work. - pcd_list = list(map(list, point_cloud2.read_points(pcd, skip_nans=True))) + pcd_list = list(map( + list, + point_cloud2.read_points(pcd, skip_nans=True))) points_nonan = points[~np.any(np.isnan(points), axis=1)] # Check correct shape self.assertTrue(np.array(pcd_list).shape == points_nonan.shape) @@ -117,12 +167,39 @@ def test_read_points_list(self): # Check that reading a PointCloud2 message to a list is performed # correctly. points_named = point_cloud2.read_points_list(pcd) - self.assertTrue(np.allclose(np.array(points_named), points, equal_nan=True)) + self.assertTrue(np.allclose( + np.array(points_named), points, equal_nan=True)) + + def test_read_points_organized(self): + # Checks if organized clouds are handled correctly + # Test if it is converted into a unorganized one by default + pcd_list = list(map(list, point_cloud2.read_points(pcd3))) + self.assertTrue(np.allclose(pcd_list, pylist, equal_nan=True)) + # Test if organization is correctly if requested + pcd_points = point_cloud2.read_points( + pcd3, reshape_organized_cloud=True) + # Because we have a 2d array of points now it is easier to cat it into a + # unstructured NumPy array instead of converting it into a list of lists + pcd_points = structured_to_unstructured(pcd_points) + self.assertTrue(np.allclose(pcd_points, points3, equal_nan=True)) + + def test_read_points_numpy(self): + # Checks if the deserialization to an unstructured numpy array works + pcd_points = point_cloud2.read_points_numpy(pcd) + self.assertTrue(np.allclose(pcd_points, points, equal_nan=True)) + + def test_read_points_different_types(self): + # Checks if the deserialization to an unstructured numpy array works + pcd_points = point_cloud2.read_points(pcd4) + self.assertTrue( + all(np.allclose(pcd_points[name], struct_points[name], equal_nan=True) + for name in struct_points.dtype.names)) + self.assertEqual(struct_points.dtype, pcd_points.dtype) def test_create_cloud(self): thispcd = point_cloud2.create_cloud(Header(frame_id='frame'), fields, pylist) - self.assertTrue(thispcd == pcd) + self.assertEqual(thispcd, pcd) thispcd = point_cloud2.create_cloud(Header(frame_id='frame2'), fields, pylist) self.assertFalse(thispcd == pcd) @@ -130,10 +207,28 @@ def test_create_cloud(self): fields2, pylist2) self.assertFalse(thispcd == pcd) + def test_create_cloud_different_types(self): + # Check if we are able to create a point cloud with different data + thispcd = point_cloud2.create_cloud( + Header(frame_id='frame'), + struct_points_fields, + struct_points) + self.assertEqual(thispcd, pcd4) + + def test_create_cloud_xyz32_organized(self): + # Checks if organized clouds are handled correctly + thispcd = point_cloud2.create_cloud_xyz32( + Header(frame_id='frame'), + points3) + print(thispcd) + print(pcd3) + self.assertEqual(thispcd, pcd3) + def test_create_cloud_xyz32(self): - thispcd = point_cloud2.create_cloud_xyz32(Header(frame_id='frame'), - pylist) - self.assertTrue(thispcd == pcd) + thispcd = point_cloud2.create_cloud_xyz32( + Header(frame_id='frame'), + pylist) + self.assertEqual(thispcd, pcd) if __name__ == '__main__': From 977afd189154dc7c04910ab707884ca3577b64ee Mon Sep 17 00:00:00 2001 From: Florian Vahl Date: Wed, 9 Mar 2022 22:34:26 +0100 Subject: [PATCH 12/20] Fix issues discorvered while testing Signed-off-by: Florian Vahl --- sensor_msgs_py/sensor_msgs_py/point_cloud2.py | 17 ++++------------- 1 file changed, 4 insertions(+), 13 deletions(-) diff --git a/sensor_msgs_py/sensor_msgs_py/point_cloud2.py b/sensor_msgs_py/sensor_msgs_py/point_cloud2.py index e0f4e05c..d1a2fa65 100644 --- a/sensor_msgs_py/sensor_msgs_py/point_cloud2.py +++ b/sensor_msgs_py/sensor_msgs_py/point_cloud2.py @@ -238,18 +238,9 @@ def create_cloud( points, dtype=dtype_from_fields(fields)) else: - assert len(points.dtype.names) == len(fields), \ - "The number of fields in the structured NumPy \ - array and the PointFields do not match!" - - all_fields_have_matching_datatypes = all(map( - lambda i: points.dtype.formats[i] == _DATATYPES[fields[i].datatype].str, - range(len(fields)))) - assert all_fields_have_matching_datatypes, \ - "PointField and structured NumPy array do not match data types for all fields! \ - Check their order and types." - # TODO check if that is the proper way to rename this - points.dtype.names = [field.name for field in fields] + assert points.dtype == dtype_from_fields(fields), \ + "PointFields and structured NumPy array dtype do not match for all fields! \ + Check their field order, names and types." else: # Cast python objects to structured NumPy array (slow) points = np.array( @@ -276,7 +267,7 @@ def create_cloud( is_bigendian=sys.byteorder != 'little', fields=fields, point_step=points.dtype.itemsize, - row_step=(points.dtype.itemsize * len(points)) // height, + row_step=(points.dtype.itemsize * width), data=points.tobytes()) From 962aa08b45614c9325e08853ffbcb23b06bd74ae Mon Sep 17 00:00:00 2001 From: Florian Vahl Date: Thu, 10 Mar 2022 00:52:53 +0100 Subject: [PATCH 13/20] Fix typo Signed-off-by: Florian Vahl --- sensor_msgs_py/sensor_msgs_py/point_cloud2.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/sensor_msgs_py/sensor_msgs_py/point_cloud2.py b/sensor_msgs_py/sensor_msgs_py/point_cloud2.py index d1a2fa65..5c7a1bf0 100644 --- a/sensor_msgs_py/sensor_msgs_py/point_cloud2.py +++ b/sensor_msgs_py/sensor_msgs_py/point_cloud2.py @@ -85,7 +85,7 @@ def read_points( dtype=dtype_from_fields(cloud.fields), buffer=cloud.data) - # Keep the only requested fields + # Keep only the requested fields if field_names is not None: assert all(field_name in points.dtype.names for field_name in field_names), \ "Requests field is not in the fields of the PointCloud!" From 86577e5959c0606061eab750e444321ae578dac6 Mon Sep 17 00:00:00 2001 From: Florian Vahl Date: Thu, 10 Mar 2022 01:06:22 +0100 Subject: [PATCH 14/20] Fix docstring Signed-off-by: Florian Vahl --- sensor_msgs_py/sensor_msgs_py/point_cloud2.py | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/sensor_msgs_py/sensor_msgs_py/point_cloud2.py b/sensor_msgs_py/sensor_msgs_py/point_cloud2.py index 5c7a1bf0..278e83bb 100644 --- a/sensor_msgs_py/sensor_msgs_py/point_cloud2.py +++ b/sensor_msgs_py/sensor_msgs_py/point_cloud2.py @@ -74,6 +74,7 @@ def read_points( (Type: Bool, Default: False) :param uvs: If specified, then only return the points at the given coordinates. (Type: Iterable, Default: None) + :param reshape_organized_cloud: Returns the array as an 2D organized point cloud if set. :return: Structured NumPy array containing all points. """ assert isinstance(cloud, PointCloud2), \ @@ -129,7 +130,7 @@ def read_points_numpy( uvs: Optional[Iterable] = None, reshape_organized_cloud: bool = False) -> np.ndarray: """ - Read equally typed fields from sensor_msgs.PointCloud2 message as a unstructured numpy array. + Read equally typed fields from sensor_msgs.PointCloud2 message as a unstructured numpy array. This method is better suited if one wants to perform build math operations on e.g. all x,y,z fields. @@ -143,6 +144,7 @@ def read_points_numpy( (Type: Bool, Default: False) :param uvs: If specified, then only return the points at the given coordinates. (Type: Iterable, Default: None) + :param reshape_organized_cloud: Returns the array as an 2D organized point cloud if set. :return: Numpy array containing all points. """ assert all(cloud.fields[0].datatype == field.datatype for field in cloud.fields[1:]), \ From ff9e8e25aa8812cfa797f8946a3d35e0b45c53d5 Mon Sep 17 00:00:00 2001 From: Florian Vahl Date: Thu, 10 Mar 2022 01:08:31 +0100 Subject: [PATCH 15/20] Fix docstring Signed-off-by: Florian Vahl --- sensor_msgs_py/sensor_msgs_py/point_cloud2.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/sensor_msgs_py/sensor_msgs_py/point_cloud2.py b/sensor_msgs_py/sensor_msgs_py/point_cloud2.py index 278e83bb..b9b0f856 100644 --- a/sensor_msgs_py/sensor_msgs_py/point_cloud2.py +++ b/sensor_msgs_py/sensor_msgs_py/point_cloud2.py @@ -132,7 +132,7 @@ def read_points_numpy( """ Read equally typed fields from sensor_msgs.PointCloud2 message as a unstructured numpy array. - This method is better suited if one wants to perform build math operations + This method is better suited if one wants to perform math operations on e.g. all x,y,z fields. But it is limited to fields with the same dtype as unstructured numpy arrays only contain one dtype. From 2851be5fdf2a86036dcadd1b89a65bc20ca1a724 Mon Sep 17 00:00:00 2001 From: Florian Vahl Date: Thu, 10 Mar 2022 01:22:36 +0100 Subject: [PATCH 16/20] Fix grammar Signed-off-by: Florian Vahl --- sensor_msgs_py/sensor_msgs_py/point_cloud2.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/sensor_msgs_py/sensor_msgs_py/point_cloud2.py b/sensor_msgs_py/sensor_msgs_py/point_cloud2.py index b9b0f856..6a8a384c 100644 --- a/sensor_msgs_py/sensor_msgs_py/point_cloud2.py +++ b/sensor_msgs_py/sensor_msgs_py/point_cloud2.py @@ -256,7 +256,7 @@ def create_cloud( Points can only be organized in max. two dimensional space" height = 1 width = points.shape[0] - # Check if input points where an organized cloud (2D array of points) + # Check if input points are an organized cloud (2D array of points) if len(points.shape) == 2: height = points.shape[1] From f50125defd0ea174e0d427d19646c06bb4279b4b Mon Sep 17 00:00:00 2001 From: Florian Vahl Date: Thu, 10 Mar 2022 17:49:56 +0100 Subject: [PATCH 17/20] Add support for fields with count > 1 Signed-off-by: Florian Vahl --- sensor_msgs_py/sensor_msgs_py/point_cloud2.py | 31 ++++++++++--- sensor_msgs_py/test/test_point_cloud2.py | 46 +++++++++++++++++-- 2 files changed, 67 insertions(+), 10 deletions(-) diff --git a/sensor_msgs_py/sensor_msgs_py/point_cloud2.py b/sensor_msgs_py/sensor_msgs_py/point_cloud2.py index 6a8a384c..13f81376 100644 --- a/sensor_msgs_py/sensor_msgs_py/point_cloud2.py +++ b/sensor_msgs_py/sensor_msgs_py/point_cloud2.py @@ -57,6 +57,8 @@ _DATATYPES[PointField.FLOAT32] = np.dtype(np.float32) _DATATYPES[PointField.FLOAT64] = np.dtype(np.float64) +DUMMY_FIELD_PREFIX = "unnamed_field" + def read_points( cloud: PointCloud2, @@ -194,21 +196,38 @@ def dtype_from_fields(fields: Iterable[PointField]) -> np.dtype: (Type: iterable of sensor_msgs.msg.PointField) :returns: NumPy datatype """ - # Create a list containing the names of all fields + # Create a lists containing the names, offsets and datatypes of all fields field_names = [] + field_offsets = [] + field_datatypes = [] for i, field in enumerate(fields): + # Datatype as numpy datatype + datatype = _DATATYPES[field.datatype] + # Name field if field.name == "": - name = f"unnamed_field_{i}" + name = f"{DUMMY_FIELD_PREFIX}_{i}" else: name = field.name - assert name not in field_names, "Duplicate field names are not allowed!" - field_names.append(name) + # Handle fields with count > 1 by creating subfields with a suffix consiting + # of "_" followed by the subfield counter [0 -> (count - 1)] + assert field.count > 0, "Can't process fields with count = 0." + for a in range(field.count): + # Add suffix if we have multiple subfields + if field.count > 1: + subfield_name = f"{name}_{a}" + else: + subfield_name = name + assert subfield_name not in field_names, "Duplicate field names are not allowed!" + field_names.append(subfield_name) + # Create new offset that includes subfields + 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': [_DATATYPES[field.datatype].str for field in fields], - 'offsets': [field.offset for field in fields], + 'formats': field_datatypes, + 'offsets': field_offsets, }) diff --git a/sensor_msgs_py/test/test_point_cloud2.py b/sensor_msgs_py/test/test_point_cloud2.py index b0594c57..3026f7e4 100644 --- a/sensor_msgs_py/test/test_point_cloud2.py +++ b/sensor_msgs_py/test/test_point_cloud2.py @@ -130,6 +130,25 @@ data=struct_points.tobytes() ) +# Point cloud with a field with count > 1 +count = 3 +fields5 = [PointField(name='x', offset=0, datatype=PointField.FLOAT32, count=3)] + +itemsize = points.itemsize * count +data = points.tobytes() + +pcd5 = PointCloud2( + header=Header(frame_id='frame'), + height=1, + width=points.shape[0], + is_dense=False, + is_bigendian=sys.byteorder != 'little', + fields=fields5, + point_step=(itemsize), + row_step=(itemsize * points.shape[0]), + data=data +) + class TestPointCloud2Methods(unittest.TestCase): @@ -196,6 +215,18 @@ def test_read_points_different_types(self): for name in struct_points.dtype.names)) self.assertEqual(struct_points.dtype, pcd_points.dtype) + def test_read_points_non_one_count(self): + pcd_points = point_cloud2.read_points_numpy(pcd5) + self.assertTrue( + np.allclose(pcd_points, points, equal_nan=True)) + + def test_read_points_non_one_count_structured(self): + pcd_points = point_cloud2.read_points(pcd5) + pcd_points_unstructured = structured_to_unstructured(pcd_points) + self.assertTrue( + np.allclose(pcd_points_unstructured, points, equal_nan=True)) + self.assertEqual(pcd_points.dtype.names, ("x_0", "x_1", "x_2")) + def test_create_cloud(self): thispcd = point_cloud2.create_cloud(Header(frame_id='frame'), fields, pylist) @@ -215,6 +246,12 @@ def test_create_cloud_different_types(self): struct_points) self.assertEqual(thispcd, pcd4) + def test_create_cloud_xyz32(self): + thispcd = point_cloud2.create_cloud_xyz32( + Header(frame_id='frame'), + pylist) + self.assertEqual(thispcd, pcd) + def test_create_cloud_xyz32_organized(self): # Checks if organized clouds are handled correctly thispcd = point_cloud2.create_cloud_xyz32( @@ -224,11 +261,12 @@ def test_create_cloud_xyz32_organized(self): print(pcd3) self.assertEqual(thispcd, pcd3) - def test_create_cloud_xyz32(self): - thispcd = point_cloud2.create_cloud_xyz32( + def test_create_cloud__non_one_count(self): + thispcd = point_cloud2.create_cloud( Header(frame_id='frame'), - pylist) - self.assertEqual(thispcd, pcd) + fields5, + points) + self.assertEqual(thispcd, pcd5) if __name__ == '__main__': From e0113971c493d22679f997783bf0b747367300f0 Mon Sep 17 00:00:00 2001 From: Florian Vahl Date: Tue, 15 Mar 2022 15:55:06 +0100 Subject: [PATCH 18/20] Use casting suggested by apockill Signed-off-by: Florian Vahl --- sensor_msgs_py/sensor_msgs_py/point_cloud2.py | 16 +++++++++++++--- 1 file changed, 13 insertions(+), 3 deletions(-) diff --git a/sensor_msgs_py/sensor_msgs_py/point_cloud2.py b/sensor_msgs_py/sensor_msgs_py/point_cloud2.py index 13f81376..b8d6a94b 100644 --- a/sensor_msgs_py/sensor_msgs_py/point_cloud2.py +++ b/sensor_msgs_py/sensor_msgs_py/point_cloud2.py @@ -37,6 +37,7 @@ sensor_msgs/src/sensor_msgs/point_cloud2.py """ +import array import sys from collections import namedtuple from typing import Iterable, List, NamedTuple, Optional @@ -279,8 +280,14 @@ def create_cloud( if len(points.shape) == 2: height = points.shape[1] + # Convert numpy points to array.array + memory_view = memoryview(points) + casted = memory_view.cast("B") + array_array = array.array("B") + array_array.frombytes(casted) + # Put everything together - return PointCloud2( + cloud = PointCloud2( header=header, height=height, width=width, @@ -288,8 +295,11 @@ def create_cloud( is_bigendian=sys.byteorder != 'little', fields=fields, point_step=points.dtype.itemsize, - row_step=(points.dtype.itemsize * width), - data=points.tobytes()) + row_step=(points.dtype.itemsize * width)) + # Set cloud via property instead of the constructor because of the bug described in + # https://github.com/ros2/common_interfaces/issues/176 + cloud.data = array_array + return cloud def create_cloud_xyz32(header: Header, points: Iterable) -> PointCloud2: From e472ed58f56e86fae335e7390f0380bc5276be76 Mon Sep 17 00:00:00 2001 From: Florian Vahl Date: Sun, 27 Mar 2022 20:17:58 +0200 Subject: [PATCH 19/20] Fix flake8 errors Signed-off-by: Florian Vahl --- sensor_msgs_py/sensor_msgs_py/point_cloud2.py | 37 +++++++++---------- sensor_msgs_py/test/test_point_cloud2.py | 8 ++-- 2 files changed, 22 insertions(+), 23 deletions(-) diff --git a/sensor_msgs_py/sensor_msgs_py/point_cloud2.py b/sensor_msgs_py/sensor_msgs_py/point_cloud2.py index b8d6a94b..ef22a93f 100644 --- a/sensor_msgs_py/sensor_msgs_py/point_cloud2.py +++ b/sensor_msgs_py/sensor_msgs_py/point_cloud2.py @@ -38,15 +38,14 @@ """ import array -import sys from collections import namedtuple -from typing import Iterable, List, NamedTuple, Optional - import numpy as np from numpy.lib.recfunctions import (structured_to_unstructured, unstructured_to_structured) +import sys from sensor_msgs.msg import PointCloud2, PointField from std_msgs.msg import Header +from typing import Iterable, List, NamedTuple, Optional _DATATYPES = {} _DATATYPES[PointField.INT8] = np.dtype(np.int8) @@ -58,7 +57,7 @@ _DATATYPES[PointField.FLOAT32] = np.dtype(np.float32) _DATATYPES[PointField.FLOAT64] = np.dtype(np.float64) -DUMMY_FIELD_PREFIX = "unnamed_field" +DUMMY_FIELD_PREFIX = 'unnamed_field' def read_points( @@ -92,12 +91,12 @@ def read_points( # Keep only the requested fields if field_names is not None: assert all(field_name in points.dtype.names for field_name in field_names), \ - "Requests field is not in the fields of the PointCloud!" + 'Requests field is not in the fields of the PointCloud!' # Mask fields points = points[list(field_names)] # Swap array if byte order does not match - if bool(sys.byteorder != "little") != bool(cloud.is_bigendian): + if bool(sys.byteorder != 'little') != bool(cloud.is_bigendian): points = points.byteswap(inplace=True) # Check if we want to drop points with nan values @@ -151,7 +150,7 @@ def read_points_numpy( :return: Numpy array containing all points. """ assert all(cloud.fields[0].datatype == field.datatype for field in cloud.fields[1:]), \ - "All fields need to have the same datatype. Use `read_points()` otherwise." + 'All fields need to have the same datatype. Use `read_points()` otherwise.' structured_numpy_array = read_points( cloud, field_names, skip_nans, uvs, reshape_organized_cloud) return structured_to_unstructured(structured_numpy_array) @@ -205,8 +204,8 @@ def dtype_from_fields(fields: Iterable[PointField]) -> np.dtype: # Datatype as numpy datatype datatype = _DATATYPES[field.datatype] # Name field - if field.name == "": - name = f"{DUMMY_FIELD_PREFIX}_{i}" + if field.name == '': + name = f'{DUMMY_FIELD_PREFIX}_{i}' else: name = field.name # Handle fields with count > 1 by creating subfields with a suffix consiting @@ -215,10 +214,10 @@ def dtype_from_fields(fields: Iterable[PointField]) -> np.dtype: for a in range(field.count): # Add suffix if we have multiple subfields if field.count > 1: - subfield_name = f"{name}_{a}" + subfield_name = f'{name}_{a}' else: subfield_name = name - assert subfield_name not in field_names, "Duplicate field names are not allowed!" + assert subfield_name not in field_names, 'Duplicate field names are not allowed!' field_names.append(subfield_name) # Create new offset that includes subfields field_offsets.append(field.offset + a * datatype.itemsize) @@ -253,16 +252,16 @@ def create_cloud( # Check if this is an unstructured array if points.dtype.names is None: assert all(fields[0].datatype == field.datatype for field in fields[1:]), \ - "All fields need to have the same datatype. Pass a structured NumPy array \ - with multiple dtypes otherwise." + 'All fields need to have the same datatype. Pass a structured NumPy array \ + with multiple dtypes otherwise.' # Convert unstructured to structured array points = unstructured_to_structured( points, dtype=dtype_from_fields(fields)) else: assert points.dtype == dtype_from_fields(fields), \ - "PointFields and structured NumPy array dtype do not match for all fields! \ - Check their field order, names and types." + 'PointFields and structured NumPy array dtype do not match for all fields! \ + Check their field order, names and types.' else: # Cast python objects to structured NumPy array (slow) points = np.array( @@ -272,8 +271,8 @@ def create_cloud( # Handle organized clouds assert len(points.shape) <= 2, \ - "Too many dimensions for organized cloud! \ - Points can only be organized in max. two dimensional space" + 'Too many dimensions for organized cloud! \ + Points can only be organized in max. two dimensional space' height = 1 width = points.shape[0] # Check if input points are an organized cloud (2D array of points) @@ -282,8 +281,8 @@ def create_cloud( # Convert numpy points to array.array memory_view = memoryview(points) - casted = memory_view.cast("B") - array_array = array.array("B") + casted = memory_view.cast('B') + array_array = array.array('B') array_array.frombytes(casted) # Put everything together diff --git a/sensor_msgs_py/test/test_point_cloud2.py b/sensor_msgs_py/test/test_point_cloud2.py index 3026f7e4..95aa09e6 100644 --- a/sensor_msgs_py/test/test_point_cloud2.py +++ b/sensor_msgs_py/test/test_point_cloud2.py @@ -104,9 +104,9 @@ # Make each point a tuple list(map(tuple, points)), dtype=[ - ("a", np.float32), - ("b", np.float64), - ("c", np.uint8), + ('a', np.float32), + ('b', np.float64), + ('c', np.uint8), ]) struct_points_itemsize = struct_points.itemsize @@ -225,7 +225,7 @@ def test_read_points_non_one_count_structured(self): pcd_points_unstructured = structured_to_unstructured(pcd_points) self.assertTrue( np.allclose(pcd_points_unstructured, points, equal_nan=True)) - self.assertEqual(pcd_points.dtype.names, ("x_0", "x_1", "x_2")) + self.assertEqual(pcd_points.dtype.names, ('x_0', 'x_1', 'x_2')) def test_create_cloud(self): thispcd = point_cloud2.create_cloud(Header(frame_id='frame'), From 2d0c85a69457f72c8ccc7bed8644827e2246c5ed Mon Sep 17 00:00:00 2001 From: Florian Vahl Date: Mon, 28 Mar 2022 08:16:09 +0200 Subject: [PATCH 20/20] Fix import order Signed-off-by: Florian Vahl --- sensor_msgs_py/sensor_msgs_py/point_cloud2.py | 6 ++++-- 1 file changed, 4 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 ef22a93f..55c37fcf 100644 --- a/sensor_msgs_py/sensor_msgs_py/point_cloud2.py +++ b/sensor_msgs_py/sensor_msgs_py/point_cloud2.py @@ -39,13 +39,15 @@ import array from collections import namedtuple +import sys +from typing import Iterable, List, NamedTuple, Optional + import numpy as np from numpy.lib.recfunctions import (structured_to_unstructured, unstructured_to_structured) -import sys from sensor_msgs.msg import PointCloud2, PointField from std_msgs.msg import Header -from typing import Iterable, List, NamedTuple, Optional + _DATATYPES = {} _DATATYPES[PointField.INT8] = np.dtype(np.int8)