point_cloud2.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
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


sensor_msgs
Author(s): Maintained by Tully Foote/tfoote@willowgarage.com
autogenerated on Sat Dec 28 2013 16:52:51