00001 PKG = 'posedetectiondb'
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
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