00001 from sensor_msgs.msg import PointCloud2
00002 import numpy as np
00003 import struct
00004 import time
00005
00006 def get_auto(data, wants, flipit=True):
00007 resolution = (data.height, data.width)
00008 img = np.fromstring(data.data, np.float32)
00009 step=img.shape[0]/(data.height*data.width)
00010 ret = []
00011 for w, p in wants:
00012 if w=="pos":
00013 x = img[p::step].reshape(resolution)
00014 y = img[p+1::step].reshape(resolution)
00015 z = img[p+2::step].reshape(resolution)
00016 if flipit:
00017 x = np.flipud(x)
00018 y = np.flipud(y)
00019 z = np.flipud(z)
00020 ret.extend([x, y, z])
00021 elif w=="rgb":
00022 rgb = img[p::step]
00023 r = np.zeros(data.height*data.width)
00024 g = np.zeros(data.height*data.width)
00025 b = np.zeros(data.height*data.width)
00026 for i in range(rgb.shape[0]):
00027 int_rgb = struct.unpack('i', struct.pack('f', rgb[i]))[0]
00028 r[i] = (int_rgb & 0xff0000) >> 16
00029 g[i] = (int_rgb & 0xff00) >> 8
00030 b[i] = (int_rgb & 0xff)
00031 r = r.reshape(resolution)
00032 g = g.reshape(resolution)
00033 b = b.reshape(resolution)
00034 if flipit:
00035 r = np.flipud(r)
00036 g = np.flipud(g)
00037 b = np.flipud(b)
00038 gray = (0.30*r+0.59*g+0.11*b).astype('uint8')
00039 ret.extend([r,g,b,gray])
00040 elif w=="norm":
00041 n_x = img[p::step].reshape(resolution)
00042 n_y = img[p+1::step].reshape(resolution)
00043 n_z = img[p+2::step].reshape(resolution)
00044 if flipit:
00045 n_x = np.flipud(n_x)
00046 n_y = np.flipud(n_y)
00047 n_z = np.flipud(n_z)
00048 ret.extend([n_x, n_y, n_z])
00049 elif w=="curv":
00050 curv = img[p::step].reshape(resolution)
00051 if flipit:
00052 curv = np.flipud(curv)
00053 ret.extend([curv])
00054 return ret
00055
00056 def get_pos_rgb(data):
00057 resolution = (data.height, data.width)
00058
00059 img = np.fromstring(data.data, np.float32)
00060
00061 x = img[0::8].reshape(resolution)
00062 y = img[1::8].reshape(resolution)
00063 z = img[2::8].reshape(resolution)
00064 rgb = img[4::8]
00065 r = np.zeros(data.height*data.width)
00066 g = np.zeros(data.height*data.width)
00067 b = np.zeros(data.height*data.width)
00068 for i in range(rgb.shape[0]):
00069 int_rgb = struct.unpack('i', struct.pack('f', rgb[i]))[0]
00070 r[i] = (int_rgb & 0xff0000) >> 16
00071 g[i] = (int_rgb & 0xff00) >> 8
00072 b[i] = (int_rgb & 0xff)
00073 r = r.reshape(resolution)
00074 g = g.reshape(resolution)
00075 b = b.reshape(resolution)
00076 x = np.flipud(x)
00077 y = np.flipud(y)
00078 z = np.flipud(z)
00079 return (x, y, z, r, g, b)
00080
00081 def get_pos_rgb_gray(data):
00082 resolution = (data.height, data.width)
00083
00084 img = np.fromstring(data.data, np.float32)
00085
00086 x = img[0::8].reshape(resolution)
00087 y = img[1::8].reshape(resolution)
00088 z = img[2::8].reshape(resolution)
00089 rgb = img[4::8]
00090 r = np.zeros(data.height*data.width)
00091 g = np.zeros(data.height*data.width)
00092 b = np.zeros(data.height*data.width)
00093 for i in range(rgb.shape[0]):
00094 int_rgb = struct.unpack('i', struct.pack('f', rgb[i]))[0]
00095 r[i] = (int_rgb & 0xff0000) >> 16
00096 g[i] = (int_rgb & 0xff00) >> 8
00097 b[i] = (int_rgb & 0xff)
00098 r = r.reshape(resolution)
00099 g = g.reshape(resolution)
00100 b = b.reshape(resolution)
00101 x = np.flipud(x)
00102 y = np.flipud(y)
00103 z = np.flipud(z)
00104 gray = np.flipud(0.30*r+0.59*g+0.11*b).astype('uint8')
00105 return (x, y, z, r, g, b, gray)
00106
00107
00108 def get_pos_rgb_gray_norm(data):
00109 resolution = (data.height, data.width)
00110
00111 img = np.fromstring(data.data, np.float32)
00112
00113 print img.shape
00114 x = img[0::12].reshape(resolution)
00115 y = img[1::12].reshape(resolution)
00116 z = img[2::12].reshape(resolution)
00117 n_x = img[4::12].reshape(resolution)
00118 n_y = img[5::12].reshape(resolution)
00119 n_z = img[6::12].reshape(resolution)
00120 rgb = img[8::12]
00121 r = np.zeros(data.height*data.width)
00122 g = np.zeros(data.height*data.width)
00123 b = np.zeros(data.height*data.width)
00124 curv = img[9::12].reshape(resolution)
00125 for i in range(rgb.shape[0]):
00126 int_rgb = struct.unpack('i', struct.pack('f', rgb[i]))[0]
00127 r[i] = (int_rgb & 0xff0000) >> 16
00128 g[i] = (int_rgb & 0xff00) >> 8
00129 b[i] = (int_rgb & 0xff)
00130 r = r.reshape(resolution)
00131 g = g.reshape(resolution)
00132 b = b.reshape(resolution)
00133
00134 x = np.flipud(x)
00135 y = np.flipud(y)
00136 z = np.flipud(z)
00137 n_x = np.flipud(n_x)
00138 n_y = np.flipud(n_y)
00139 n_z = np.flipud(n_z)
00140 curv = np.flipud(curv)
00141 gray = np.flipud(0.30*r+0.59*g+0.11*b).astype('uint8')
00142
00143 return (x, y, z, n_x, n_y, n_z, r, g, b, curv, gray)