From a1549a33051004b04a3dd1a66af6c749ca95d3d6 Mon Sep 17 00:00:00 2001 From: Gianluca Bardaro Date: Mon, 20 Jan 2020 15:20:55 +0000 Subject: [PATCH 1/3] changed create_cloud to support ordered pointclouds --- sensor_msgs/src/sensor_msgs/point_cloud2.py | 15 +++++++++------ 1 file changed, 9 insertions(+), 6 deletions(-) diff --git a/sensor_msgs/src/sensor_msgs/point_cloud2.py b/sensor_msgs/src/sensor_msgs/point_cloud2.py index c4c1f472..43d162ec 100644 --- a/sensor_msgs/src/sensor_msgs/point_cloud2.py +++ b/sensor_msgs/src/sensor_msgs/point_cloud2.py @@ -138,7 +138,7 @@ def read_points_list(cloud, field_names=None, skip_nans=False, uvs=[]): return [Point._make(l) for l in read_points(cloud, field_names, skip_nans, uvs)] -def create_cloud(header, fields, points): +def create_cloud(header, fields, points, height=1, width=None): """ Create a L{sensor_msgs.msg.PointCloud2} message. @@ -153,25 +153,28 @@ def create_cloud(header, fields, points): @return: The point cloud. @rtype: L{sensor_msgs.msg.PointCloud2} """ + if height == 1: + width = len(points) 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 + point_step = cloud_struct.size + pack_into = 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), + height=height, + width=width, is_dense=False, is_bigendian=False, fields=fields, - point_step=cloud_struct.size, - row_step=cloud_struct.size * len(points), + point_step=point_step, + row_step=cloud_struct.size * width, data=buff.raw) def create_cloud_xyz32(header, points): From 3734da2046a3c6f19b1b69abcb7665e61bff7d75 Mon Sep 17 00:00:00 2001 From: Gianluca Bardaro Date: Mon, 20 Jan 2020 15:24:08 +0000 Subject: [PATCH 2/3] updated documentation of create_cloud --- sensor_msgs/src/sensor_msgs/point_cloud2.py | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/sensor_msgs/src/sensor_msgs/point_cloud2.py b/sensor_msgs/src/sensor_msgs/point_cloud2.py index 43d162ec..5b3ac250 100644 --- a/sensor_msgs/src/sensor_msgs/point_cloud2.py +++ b/sensor_msgs/src/sensor_msgs/point_cloud2.py @@ -150,6 +150,10 @@ def create_cloud(header, fields, points, height=1, width=None): @type 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) + @param height: the height of the point cloud [default: 1] + @type height: unsigned int + @param width: the width of the point cloud [default: None] + @param width: unsigned int @return: The point cloud. @rtype: L{sensor_msgs.msg.PointCloud2} """ From 6121f9ca14e7479392210daf2d20b48b899b408e Mon Sep 17 00:00:00 2001 From: Gianluca Bardaro Date: Tue, 28 Jan 2020 16:06:35 +0000 Subject: [PATCH 3/3] check on both height and width --- sensor_msgs/src/sensor_msgs/point_cloud2.py | 11 +++++------ 1 file changed, 5 insertions(+), 6 deletions(-) diff --git a/sensor_msgs/src/sensor_msgs/point_cloud2.py b/sensor_msgs/src/sensor_msgs/point_cloud2.py index 5b3ac250..3bdabe27 100644 --- a/sensor_msgs/src/sensor_msgs/point_cloud2.py +++ b/sensor_msgs/src/sensor_msgs/point_cloud2.py @@ -157,19 +157,18 @@ def create_cloud(header, fields, points, height=1, width=None): @return: The point cloud. @rtype: L{sensor_msgs.msg.PointCloud2} """ - if height == 1: + if height == 1 or width is None: + height = 1 width = len(points) cloud_struct = struct.Struct(_get_struct_fmt(False, fields)) buff = ctypes.create_string_buffer(cloud_struct.size * len(points)) - point_step = cloud_struct.size - pack_into = cloud_struct.pack_into offset = 0 for p in points: - pack_into(buff, offset, *p) - offset += point_step + cloud_struct.pack_into(buff, offset, *p) + offset += cloud_struct.size return PointCloud2(header=header, height=height, @@ -177,7 +176,7 @@ def create_cloud(header, fields, points, height=1, width=None): is_dense=False, is_bigendian=False, fields=fields, - point_step=point_step, + point_step=cloud_struct.size, row_step=cloud_struct.size * width, data=buff.raw)