point_cloud2.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 
00003 # Software License Agreement (BSD License)
00004 #
00005 # Copyright (c) 208, Willow Garage, Inc.
00006 # All rights reserved.
00007 #
00008 # Redistribution and use in source and binary forms, with or without
00009 # modification, are permitted provided that the following conditions
00010 # are met:
00011 #
00012 # * Redistributions of source code must retain the above copyright
00013 # notice, this list of conditions and the following disclaimer.
00014 # * Redistributions in binary form must reproduce the above
00015 # copyright notice, this list of conditions and the following
00016 # disclaimer in the documentation and/or other materials provided
00017 # with the distribution.
00018 # * Neither the name of Willow Garage, Inc. nor the names of its
00019 # contributors may be used to endorse or promote products derived
00020 # from this software without specific prior written permission.
00021 #
00022 # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00023 # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00024 # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00025 # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00026 # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00027 # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00028 # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00029 # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00030 # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00031 # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00032 # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00033 # POSSIBILITY OF SUCH DAMAGE.
00034 
00035 """
00036 Serialization of sensor_msgs.PointCloud2 messages.
00037 
00038 Author: Tim Field
00039 """
00040 
00041 import ctypes
00042 import math
00043 import struct
00044 
00045 import roslib.message
00046 from sensor_msgs.msg import PointCloud2, PointField
00047 
00048 _DATATYPES = {}
00049 _DATATYPES[PointField.INT8]    = ('b', 1)
00050 _DATATYPES[PointField.UINT8]   = ('B', 1)
00051 _DATATYPES[PointField.INT16]   = ('h', 2)
00052 _DATATYPES[PointField.UINT16]  = ('H', 2)
00053 _DATATYPES[PointField.INT32]   = ('i', 4)
00054 _DATATYPES[PointField.UINT32]  = ('I', 4)
00055 _DATATYPES[PointField.FLOAT32] = ('f', 4)
00056 _DATATYPES[PointField.FLOAT64] = ('d', 8)
00057 
00058 def read_points(cloud, field_names=None, skip_nans=False, uvs=[]):
00059     """
00060     Read points from a L{sensor_msgs.PointCloud2} message.
00061 
00062     @param cloud: The point cloud to read from.
00063     @type  cloud: L{sensor_msgs.PointCloud2}
00064     @param field_names: The names of fields to read. If None, read all fields. [default: None]
00065     @type  field_names: iterable
00066     @param skip_nans: If True, then don't return any point with a NaN value.
00067     @type  skip_nans: bool [default: False]
00068     @param uvs: If specified, then only return the points at the given coordinates. [default: empty list]
00069     @type  uvs: iterable
00070     @return: Generator which yields a list of values for each point.
00071     @rtype:  generator
00072     """
00073     assert isinstance(cloud, roslib.message.Message) and cloud._type == 'sensor_msgs/PointCloud2', 'cloud is not a sensor_msgs.msg.PointCloud2'
00074     fmt = _get_struct_fmt(cloud.is_bigendian, cloud.fields, field_names)
00075     width, height, point_step, row_step, data, isnan = cloud.width, cloud.height, cloud.point_step, cloud.row_step, cloud.data, math.isnan
00076     unpack_from = struct.Struct(fmt).unpack_from
00077 
00078     if skip_nans:
00079         if uvs:
00080             for u, v in uvs:
00081                 p = unpack_from(data, (row_step * v) + (point_step * u))
00082                 has_nan = False
00083                 for pv in p:
00084                     if isnan(pv):
00085                         has_nan = True
00086                         break
00087                 if not has_nan:
00088                     yield p
00089         else:
00090             for v in xrange(height):
00091                 offset = row_step * v
00092                 for u in xrange(width):
00093                     p = unpack_from(data, offset)
00094                     has_nan = False
00095                     for pv in p:
00096                         if isnan(pv):
00097                             has_nan = True
00098                             break
00099                     if not has_nan:
00100                         yield p
00101                     offset += point_step
00102     else:
00103         if uvs:
00104             for u, v in uvs:
00105                 yield unpack_from(data, (row_step * v) + (point_step * u))
00106         else:
00107             for v in xrange(height):
00108                 offset = row_step * v
00109                 for u in xrange(width):
00110                     yield unpack_from(data, offset)
00111                     offset += point_step
00112 
00113 def create_cloud(header, fields, points):
00114     """
00115     Create a L{sensor_msgs.msg.PointCloud2} message.
00116 
00117     @param header: The point cloud header.
00118     @type  header: L{std_msgs.msg.Header}
00119     @param fields: The point cloud fields.
00120     @type  fields: iterable of L{sensor_msgs.msg.PointField}
00121     @param points: The point cloud points.
00122     @type  points: list of iterables, i.e. one iterable for each point, with the
00123                    elements of each iterable being the values of the fields for 
00124                    that point (in the same order as the fields parameter)
00125     @return: The point cloud.
00126     @rtype:  L{sensor_msgs.msg.PointCloud2}
00127     """
00128 
00129     cloud_struct = struct.Struct(_get_struct_fmt(False, fields))
00130 
00131     buff = ctypes.create_string_buffer(cloud_struct.size * len(points))
00132 
00133     point_step, pack_into = cloud_struct.size, cloud_struct.pack_into
00134     offset = 0
00135     for p in points:
00136         pack_into(buff, offset, *p)
00137         offset += point_step
00138 
00139     return PointCloud2(header=header,
00140                        height=1,
00141                        width=len(points),
00142                        is_dense=False,
00143                        is_bigendian=False,
00144                        fields=fields,
00145                        point_step=cloud_struct.size,
00146                        row_step=cloud_struct.size * len(points),
00147                        data=buff.raw)
00148 
00149 def create_cloud_xyz32(header, points):
00150     """
00151     Create a L{sensor_msgs.msg.PointCloud2} message with 3 float32 fields (x, y, z).
00152 
00153     @param header: The point cloud header.
00154     @type  header: L{std_msgs.msg.Header}
00155     @param points: The point cloud points.
00156     @type  points: iterable
00157     @return: The point cloud.
00158     @rtype:  L{sensor_msgs.msg.PointCloud2}
00159     """
00160     fields = [PointField('x', 0, PointField.FLOAT32, 1),
00161               PointField('y', 4, PointField.FLOAT32, 1),
00162               PointField('z', 8, PointField.FLOAT32, 1)]
00163     return create_cloud(header, fields, points)
00164 
00165 def _get_struct_fmt(is_bigendian, fields, field_names=None):
00166     fmt = '>' if is_bigendian else '<'
00167 
00168     offset = 0
00169     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):
00170         if offset < field.offset:
00171             fmt += 'x' * (field.offset - offset)
00172             offset = field.offset
00173         if field.datatype not in _DATATYPES:
00174             print >> sys.stderr, 'Skipping unknown PointField datatype [%d]' % field.datatype
00175         else:
00176             datatype_fmt, datatype_length = _DATATYPES[field.datatype]
00177             fmt    += field.count * datatype_fmt
00178             offset += field.count * datatype_length
00179 
00180     return fmt


sensor_msgs
Author(s):
autogenerated on Sun Oct 5 2014 23:11:21