ParseMessages.py
Go to the documentation of this file.
00001 PKG = 'posedetectiondb' # this package name
00002 import roslib; roslib.load_manifest(PKG) 
00003 import os, sys, signal, time, threading, struct, string
00004 import numpy, scipy
00005 import PIL.Image
00006 
00007 import rospy
00008 import sensor_msgs.msg
00009 
00010 # Number of channels used by a PIL image mode
00011 def imgmsg_to_pil(rosimage, encoding_to_mode = {
00012         'mono8' :     'L',
00013         '8UC1' :      'L',
00014         '8UC3' :      'RGB',
00015         'rgb8':       'RGB',
00016         'bgr8':       'RGB',
00017         'rgba8':      'RGBA',
00018         'bgra8':      'RGBA',
00019         'bayer_rggb': 'L',
00020         'bayer_gbrg': 'L',
00021         'bayer_grbg': 'L',
00022         'bayer_bggr': 'L',
00023         'yuv422':     'YCbCr',
00024         'yuv411':     'YCbCr'}, PILmode_channels = { 'L' : 1, 'RGB' : 3, 'RGBA' : 4, 'YCbCr' : 3 }):
00025     conversion = 'B'
00026     channels = 1
00027     if rosimage.encoding.find('32FC') >= 0:
00028         conversion = 'f'
00029         channels = int(rosimage.encoding[-1])
00030     elif rosimage.encoding.find('64FC') >= 0:
00031         conversion = 'd'
00032         channels = int(rosimage.encoding[-1])
00033     elif rosimage.encoding.find('8SC') >= 0:
00034         conversion = 'b'
00035         channels = int(rosimage.encoding[-1])
00036     elif rosimage.encoding.find('8UC') >= 0:
00037         conversion = 'B'
00038         channels = int(rosimage.encoding[-1])
00039     elif rosimage.encoding.find('16UC') >= 0:
00040         conversion = 'H'
00041         channels = int(rosimage.encoding[-1])
00042     elif rosimage.encoding.find('16SC') >= 0:
00043         conversion = 'h'
00044         channels = int(rosimage.encoding[-1])
00045     elif rosimage.encoding.find('32UC') >= 0:
00046         conversion = 'I'
00047         channels = int(rosimage.encoding[-1])
00048     elif rosimage.encoding.find('32SC') >= 0:
00049         conversion = 'i'
00050         channels = int(rosimage.encoding[-1])
00051     else:
00052         if rosimage.encoding.find('rgb') >= 0 or rosimage.encoding.find('bgr') >= 0:
00053             channels = 3
00054 
00055     data = struct.unpack( ('>' if rosimage.is_bigendian else '<') + '%d'%(rosimage.width*rosimage.height*channels) + conversion,rosimage.data)
00056     
00057     if conversion == 'f' or conversion == 'd':
00058         dimsizes = [rosimage.height, rosimage.width, channels]
00059         imagearr = numpy.array(255*I,dtype=numpy.uint8)
00060         im = PIL.Image.frombuffer('RGB' if channels == 3 else 'L',dimsizes[1::-1],imagearr.tostring(), 'raw','RGB',0,1)
00061         if channels == 3:
00062             im = PIL.Image.merge('RGB',im.split()[-1::-1])
00063         return im,data,dimsizes
00064     else:
00065         mode = encoding_to_mode[rosimage.encoding]
00066         step_size = PILmode_channels[mode]
00067         dimsizes = [rosimage.height, rosimage.width, step_size]
00068         im = PIL.Image.frombuffer(mode,dimsizes[1::-1], rosimage.data,'raw',mode,0,1)
00069         if mode == 'RGB':
00070             im = PIL.Image.merge('RGB',im.split()[-1::-1])
00071         return im,data,dimsizes
00072 
00073 def pil_to_imgmsg(image,encodingmap={'L':'mono8', 'RGB':'rgb8','RGBA':'rgba8', 'YCbCr':'yuv422'},
00074                         PILmode_channels = { 'L' : 1, 'RGB' : 3, 'RGBA' : 4, 'YCbCr' : 3 }):
00075     rosimage = sensor_msgs.msg.Image()
00076     rosimage.encoding = encodingmap[image.mode]
00077     (rosimage.width, rosimage.height) = image.size
00078     rosimage.step = PILmode_channels[image.mode] * rosimage.width
00079     rosimage.data = image.tostring()
00080     return rosimage
00081 
00082 def numpy_to_imgmsg(image,stamp=None):
00083     rosimage = sensor_msgs.msg.Image()
00084     rosimage.height = image.shape[0]
00085     rosimage.width = image.shape[1]
00086     if image.dtype == numpy.uint8:
00087         rosimage.encoding = '8UC%d' % image.shape[2]
00088         rosimage.step = image.shape[2] * rosimage.width
00089         rosimage.data = image.ravel().tolist()
00090     else:
00091         rosimage.encoding = '32FC%d' % image.shape[2]
00092         rosimage.step = image.shape[2] * rosimage.width * 4
00093         rosimage.data = numpy.array(image.flat,dtype=numpy.float32).tostring()
00094     if stamp is not None:
00095         rosimage.header.stamp = stamp
00096     return rosimage


posedetectiondb
Author(s): Rosen Diankov (rdiankov@cs.cmu.edu)
autogenerated on Tue Jan 27 2015 11:53:11