00001
00002 import roslib; roslib.load_manifest('sensor_msgs')
00003 import math
00004 import struct
00005 import rosbag
00006 from sensor_msgs.msg import PointField
00007
00008 _DATATYPES = {}
00009 _DATATYPES[PointField.INT8] = ('b', 1)
00010 _DATATYPES[PointField.UINT8] = ('B', 1)
00011 _DATATYPES[PointField.INT16] = ('h', 2)
00012 _DATATYPES[PointField.UINT16] = ('H', 2)
00013 _DATATYPES[PointField.INT32] = ('i', 4)
00014 _DATATYPES[PointField.UINT32] = ('I', 4)
00015 _DATATYPES[PointField.FLOAT32] = ('f', 4)
00016 _DATATYPES[PointField.FLOAT64] = ('d', 8)
00017
00018 def read_points(cloud, field_names=None, skip_nans=False):
00019 assert(cloud)
00020 fmt = _get_struct_fmt(cloud, field_names)
00021 width, height, point_step, row_step, data, unpack_from, isnan = cloud.width, cloud.height, cloud.point_step, cloud.row_step, cloud.data, struct.unpack_from, math.isnan
00022 if skip_nans:
00023 for v in xrange(height):
00024 offset = row_step * v
00025 for u in xrange(width):
00026 p = unpack_from(fmt, data, offset)
00027 has_nan = False
00028 for v in p:
00029 if isnan(v):
00030 has_nan = True
00031 break
00032 if not has_nan:
00033 yield p
00034 offset += point_step
00035 else:
00036 for v in xrange(height):
00037 offset = row_step * v
00038 for u in xrange(width):
00039 yield unpack_from(fmt, data, offset)
00040 offset += point_step
00041
00042 def _get_struct_fmt(cloud, field_names=None):
00043 fmt = '>' if cloud.is_bigendian else '<'
00044
00045 offset = 0
00046 for field in (f for f in sorted(cloud.fields, key=lambda f: f.offset) if field_names is None or f.name in field_names):
00047 if offset < field.offset:
00048 fmt += 'x' * (field.offset - offset)
00049 offset = field.offset
00050 if field.datatype not in _DATATYPES:
00051 print >> sys.stderr, 'Skipping unknown PointField datatype [%d]' % field.datatype
00052 else:
00053 datatype_fmt, datatype_length = _DATATYPES[field.datatype]
00054 fmt += field.count * datatype_fmt
00055 offset += field.count * datatype_length
00056
00057 return fmt
