Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Create ordered point cloud (issue #126) #157

Open
wants to merge 3 commits into
base: jade-devel
Choose a base branch
from
Open

Create ordered point cloud (issue #126) #157

Changes from 2 commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
a1549a3
changed create_cloud to support ordered pointclouds
Jan 20, 2020
3734da2
updated documentation of create_cloud
Jan 20, 2020
6121f9c
check on both height and width
Jan 28, 2020
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
19 changes: 13 additions & 6 deletions sensor_msgs/src/sensor_msgs/point_cloud2.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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.

Expand All @@ -150,28 +150,35 @@ def create_cloud(header, fields, points):
@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}
"""
if height == 1:
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

This looks like it will have problems if height is non unary but width is defaulted to None. Some other checks are going to be necessary.

Copy link
Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I'll add a check on both height and width.
The original implementation didn't perform any check on header, fields and points, for example, if they are empty. Do you want me to add something like that?

Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I was mostly worried about the default values messing things up. Though a little bit of sanitizing would be valuable. But we need to be careful about what error response behavior is caused by invalid data.

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
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

These intermediate values appear to only be used once so are probably not worth creating at all.

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):
Expand Down