00001
00002
00003 """
00004 Serialization of sensor_msgs.PointCloud2 messages.
00005
00006 Author: Tim Field
00007 """
00008
00009 import roslib; roslib.load_manifest('sensor_msgs')
00010
00011 import ctypes
00012 import math
00013 import struct
00014
00015 from sensor_msgs.msg import PointCloud2, PointField
00016
00017 _DATATYPES = {}
00018 _DATATYPES[PointField.INT8] = ('b', 1)
00019 _DATATYPES[PointField.UINT8] = ('B', 1)
00020 _DATATYPES[PointField.INT16] = ('h', 2)
00021 _DATATYPES[PointField.UINT16] = ('H', 2)
00022 _DATATYPES[PointField.INT32] = ('i', 4)
00023 _DATATYPES[PointField.UINT32] = ('I', 4)
00024 _DATATYPES[PointField.FLOAT32] = ('f', 4)
00025 _DATATYPES[PointField.FLOAT64] = ('d', 8)
00026
00027 def read_points(cloud, field_names=None, skip_nans=False, uvs=[]):
00028 """
00029 Read points from a L{sensor_msgs.PointCloud2} message.
00030
00031 @param cloud: The point cloud to read from.
00032 @type cloud: L{sensor_msgs.PointCloud2}
00033 @param field_names: The names of fields to read. If None, read all fields. [default: None]
00034 @type field_names: iterable
00035 @param skip_nans: If True, then don't return any point with a NaN value.
00036 @type skip_nans: bool [default: False]
00037 @param uvs: If specified, then only return the points at the given coordinates. [default: empty list]
00038 @type uvs: iterable
00039 @return: Generator which yields a list of values for each point.
00040 @rtype: generator
00041 """
00042 assert isinstance(cloud, roslib.message.Message) and cloud._type == 'sensor_msgs/PointCloud2', 'cloud is not a sensor_msgs.msg.PointCloud2'
00043 fmt = _get_struct_fmt(cloud.is_bigendian, cloud.fields, field_names)
00044 width, height, point_step, row_step, data, isnan = cloud.width, cloud.height, cloud.point_step, cloud.row_step, cloud.data, math.isnan
00045 unpack_from = struct.Struct(fmt).unpack_from
00046
00047 if skip_nans:
00048 if uvs:
00049 for u, v in uvs:
00050 p = unpack_from(data, (row_step * v) + (point_step * u))
00051 has_nan = False
00052 for pv in p:
00053 if isnan(pv):
00054 has_nan = True
00055 break
00056 if not has_nan:
00057 yield p
00058 else:
00059 for v in xrange(height):
00060 offset = row_step * v
00061 for u in xrange(width):
00062 p = unpack_from(data, offset)
00063 has_nan = False
00064 for pv in p:
00065 if isnan(pv):
00066 has_nan = True
00067 break
00068 if not has_nan:
00069 yield p
00070 offset += point_step
00071 else:
00072 if uvs:
00073 for u, v in uvs:
00074 yield unpack_from(data, (row_step * v) + (point_step * u))
00075 else:
00076 for v in xrange(height):
00077 offset = row_step * v
00078 for u in xrange(width):
00079 yield unpack_from(data, offset)
00080 offset += point_step
00081
00082 def create_cloud(header, fields, points):
00083 """
00084 Create a L{sensor_msgs.msg.PointCloud2} message.
00085
00086 @param header: The point cloud header.
00087 @type header: L{std_msgs.msg.Header}
00088 @param fields: The point cloud fields.
00089 @type fields: iterable of L{sensor_msgs.msg.PointField}
00090 @param points: The point cloud points.
00091 @type points: list of iterables, i.e. one iterable for each point, with the
00092 elements of each iterable being the values of the fields for
00093 that point (in the same order as the fields parameter)
00094 @return: The point cloud.
00095 @rtype: L{sensor_msgs.msg.PointCloud2}
00096 """
00097
00098 cloud_struct = struct.Struct(_get_struct_fmt(False, fields))
00099
00100 buff = ctypes.create_string_buffer(cloud_struct.size * len(points))
00101
00102 point_step, pack_into = cloud_struct.size, cloud_struct.pack_into
00103 offset = 0
00104 for p in points:
00105 pack_into(buff, offset, *p)
00106 offset += point_step
00107
00108 return PointCloud2(header=header,
00109 height=1,
00110 width=len(points),
00111 is_dense=False,
00112 is_bigendian=False,
00113 fields=fields,
00114 point_step=cloud_struct.size,
00115 row_step=cloud_struct.size * len(points),
00116 data=buff.raw)
00117
00118 def create_cloud_xyz32(header, points):
00119 """
00120 Create a L{sensor_msgs.msg.PointCloud2} message with 3 float32 fields (x, y, z).
00121
00122 @param header: The point cloud header.
00123 @type header: L{std_msgs.msg.Header}
00124 @param points: The point cloud points.
00125 @type points: iterable
00126 @return: The point cloud.
00127 @rtype: L{sensor_msgs.msg.PointCloud2}
00128 """
00129 fields = [PointField('x', 0, PointField.FLOAT32, 1),
00130 PointField('y', 4, PointField.FLOAT32, 1),
00131 PointField('z', 8, PointField.FLOAT32, 1)]
00132 return create_cloud(header, fields, points)
00133
00134 def _get_struct_fmt(is_bigendian, fields, field_names=None):
00135 fmt = '>' if is_bigendian else '<'
00136
00137 offset = 0
00138 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):
00139 if offset < field.offset:
00140 fmt += 'x' * (field.offset - offset)
00141 offset = field.offset
00142 if field.datatype not in _DATATYPES:
00143 print >> sys.stderr, 'Skipping unknown PointField datatype [%d]' % field.datatype
00144 else:
00145 datatype_fmt, datatype_length = _DATATYPES[field.datatype]
00146 fmt += field.count * datatype_fmt
00147 offset += field.count * datatype_length
00148
00149 return fmt