def read_point(x, y, cloud, field_names=None, skip_nans=False, uvs=[]):
assert isinstance(
cloud, roslib.message.Message
) and cloud._type == 'sensor_msgs/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
offset = row_step * y + point_step * x
return unpack_from(data, offset)
def createPointCloud2(pc, uvs):
# pc: PointCloud
# uvs: the u,v pairs associated to the pc
# transform PointCloud into an organized PointCloud2 msg
with Timer('c0'):
minu, maxu = _min_max(uvs, 0)
minv, maxv = _min_max(uvs, 1)
height = maxv - minv + 1
width = maxu - minu + 1
cnt = 0
points = []
length = len(uvs)
nan = float('nan')
nans = [nan, nan, nan]
with Timer('c1'):
for i in xrange(minu, maxu+1):
for j in xrange(minv, maxv+1):
if cnt < length and uvs[cnt][0] == i and uvs[cnt][1] == j:
points.append([pc.points[cnt].x, pc.points[cnt].y, pc.points[cnt].z])
cnt += 1
else:
points.append(nans)
with Timer('c2'):
# pack the numbers into binary array
from sensor_msgs.msg import PointCloud2, PointField
from sensor_msgs.point_cloud2 import _get_struct_fmt
fields = [PointField('x', 0, PointField.FLOAT32, 1),
PointField('y', 4, PointField.FLOAT32, 1),
PointField('z', 8, PointField.FLOAT32, 1)]
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
with Timer('c3'):
return PointCloud2(header=pc.header,
height=height & 0xffffffff,
width=width & 0xffffffff,
is_dense=False,
is_bigendian=False,
fields=fields,
point_step=cloud_struct.size,
row_step=(cloud_struct.size * width) & 0xffffffff,
data=buff.raw)
def __init__(self, cloud, field_names):
assert (
isinstance(cloud, roslib.message.Message) and cloud._type == "sensor_msgs/PointCloud2"
), "cloud is not a sensor_msgs.msg.PointCloud2"
fmt = _get_struct_fmt(cloud.is_bigendian, cloud.fields, field_names)
self.width, self.height, self.point_step, self.row_step, self.data = (
cloud.width,
cloud.height,
cloud.point_step,
cloud.row_step,
cloud.data,
)
self.unpack_from = struct.Struct(fmt).unpack_from
