00001
00002
00003 import rfid_model
00004 import lib_pfilter
00005
00006 import roslib
00007 roslib.load_manifest('rfid_behaviors')
00008 roslib.load_manifest('rfid_datacapture')
00009 roslib.load_manifest('hrl_lib')
00010 roslib.load_manifest('visualization_msgs')
00011 roslib.load_manifest('hrl_lib')
00012 roslib.load_manifest('pfilter')
00013 import rospy
00014
00015 import hrl_lib.transforms as tr
00016 import tf.transformations as tft
00017 import display_particles
00018 from display_particles import DisplayParticles
00019 from geometry_msgs.msg import Quaternion, Point
00020
00021 import rfid_datacapture.math_util as mu
00022 import visualization_msgs.msg as vm
00023 import hrl_lib.transforms as tr
00024 import hrl_lib.viz as viz
00025 vsm = viz.single_marker
00026
00027
00028 import random as rd
00029 import cPickle as pkl
00030 import numpy as np, math
00031 import pylab as pl
00032 import time
00033 import glob
00034
00035
00036
00037
00038
00039
00040 def pprint(r):
00041 print '\nLocation %d, Object %d, Trial %d' % (r['loc'], r['obj_num'], r['trial_num'])
00042
00043 if r['servo_yn']:
00044 print '\tSEARCH plus SERVO'
00045 else:
00046 print '\tSEARCH ONLY'
00047
00048 print '\tPos Reads: %d' % (r['pos_readings'])
00049 print '\tTot Reads: %d' % (r['tot_readings'])
00050 print '\tPercent Reads: %2.1f' % (r['pos_readings']*100.0/r['tot_readings'])
00051
00052 if r['best_pos'].__class__ == ''.__class__:
00053 print '\tEstimate Loc: --'
00054 else:
00055 print '\tEstimate Loc: <%2.3f, %2.3f>' % ( r['best_pos'][0], r['best_pos'][1] )
00056
00057 if r['orient_est'].__class__ == ''.__class__:
00058 print '\tEstimate Theta: --'
00059 else:
00060 print '\tEstimate Theta: %2.1f (deg)' % ( math.degrees( r['orient_est'] ))
00061
00062 if r['dxy'].__class__ == ''.__class__:
00063 print '\tDist Err (m): --'
00064 else:
00065 print '\tDist Err (m): %2.3f' % ( r['dxy'] )
00066
00067 if r['dtheta'].__class__ == ''.__class__:
00068 print '\tAng Err (deg): --'
00069 else:
00070 print '\tAng Err (deg): %2.1f' % ( math.degrees( r['dtheta'] ))
00071
00072
00073
00074
00075 print '\n\n\n'
00076
00077
00078
00079 tdb = { 0: ['OrangeMedBot',[]],
00080 1: ['TravisTVremo',[]],
00081 2: ['RedBottle ',[]],
00082 3: ['OnMetalKeys ',[]],
00083 4: ['WhiteMedsBot',[]],
00084 5: ['BlueMedsBox ',[]],
00085 6: ['TeddyBearToy',[]],
00086 7: ['CordlessPhon',[]],
00087 8: ['BlueHairBrus',[]]}
00088
00089 pts = { 0: ['BehindTree',[3.757, 6.017, 0.036]],
00090 1: ['FireplaceMantle',[5.090, 4.238, 1.514]],
00091 2: ['CircleEndTable',[5.399, 2.857, 0.607]],
00092 3: ['Couch',[3.944, 1.425, 0.527]],
00093 4: ['RectEndTable',[3.302, 0.932, 0.534]],
00094 5: ['BehindKitchenTable',[-0.339, -2.393, 0.793]],
00095 6: ['NearDishwaser',[-1.926, -0.835, 0.946]],
00096 7: ['InCupboard',[-3.257, 1.294, 1.397]],
00097 8: ['OnFilingCabinet',[-0.083, 2.332, 0.670]]}
00098
00099 def process_trialobj( trial_num, obj_num, servo_yn ):
00100 obj_name = tdb[obj_num][0]
00101 tname = obj_name.replace( ' ', '' )
00102
00103 loc = (trial_num + obj_num) % 9
00104 loc_name = pts[loc][0]
00105 loc_pos = np.array(pts[loc][1])
00106
00107
00108 fname_prefix = '/home/travis/svn/robot1/src/projects/rfid_datacapture/src/rfid_datacapture/search_cap/'
00109
00110 servo_fname = fname_prefix
00111 servo_fname += 'search_aware_home/'
00112 servo_fname += 'woot_150_'+str(trial_num)+'_tag_'+obj_name.replace(' ','')+'_servo.pkl'
00113
00114 pf_search = servo_fname.replace('_servo.pkl', '_pf_search.pkl')
00115 pf_servo = servo_fname.replace('_servo.pkl', '_pf_servo.pkl')
00116
00117
00118
00119
00120 search_reads_fname = fname_prefix
00121 search_reads_fname += 'search_aware_home/woot_150_'+str(trial_num)+'_reads.pkl'
00122 f = open( search_reads_fname, 'r' )
00123 summary_search = pkl.load( f )
00124 f.close()
00125
00126 pos_readings_search = sum([ True for p in summary_search if p.read.rssi != -1 and p.read.tagID == obj_name ])
00127 tot_readings_search = len( summary_search )
00128
00129 if pos_readings_search == 0:
00130 print '\t No results for this instance.'
00131
00132 res = { 'loc': loc,
00133 'obj_num': obj_num,
00134 'trial_num': trial_num,
00135 'pos_readings': pos_readings_search,
00136 'tot_readings':tot_readings_search,
00137 'best_pos': '--',
00138 'orient_est': '--',
00139 'dxy': '--',
00140 'dtheta': '--',
00141 'servo_yn': servo_yn,
00142 'other': { 'w_mass': 0.0,
00143 'orient_fit': 0.0 }}
00144 return False, res, None
00145
00146 f = open( pf_search )
00147 p_set_loaded = pkl.load( f )
00148 f.close()
00149
00150 pos_readings = pos_readings_search
00151 tot_readings = tot_readings_search
00152
00153
00154 if servo_yn:
00155
00156
00157 f = open( servo_fname, 'r' )
00158 summary_servo = pkl.load( f )
00159 f.close()
00160
00161 pos_readings_servo = sum([ True for p in summary_servo if p.read.rssi != -1 and p.read.tagID == obj_name ])
00162 tot_readings_servo = len( summary_servo )
00163
00164 pos_readings = pos_readings_search + pos_readings_servo
00165 tot_readings = tot_readings_search + tot_readings_servo
00166
00167
00168
00169 f = open( pf_servo )
00170 p_set_loaded = pkl.load( f )
00171 f.close()
00172
00173
00174
00175
00176
00177
00178
00179
00180
00181
00182
00183
00184
00185
00186
00187
00188
00189
00190
00191
00192
00193
00194
00195
00196 p_set = np.copy( p_set_loaded )
00197 p_set = p_set[ np.argsort( p_set[:,2] )[::-1] ]
00198 w_norm = p_set[:,2] / np.sum( p_set[:,2] )
00199 w_cum = np.cumsum( w_norm )
00200
00201
00202
00203 w_mass = w_cum[:8000][-1] * 100.0
00204 p_set = p_set[:8000]
00205
00206
00207
00208
00209
00210
00211
00212
00213
00214
00215
00216
00217
00218
00219 pf_costmap = '/home/travis/svn/robot1/src/projects/rfid_pf/src/rfid_pf/pf_costmap.pkl'
00220
00221 f = open( pf_costmap )
00222 costmap = pkl.load( f )
00223 f.close()
00224
00225 cm = costmap[ np.where( costmap[:,2] )[0] ]
00226
00227
00228
00229 def score_loc( xy ):
00230
00231 mag = np.sqrt( np.sum( np.power( p_set[:,0:2] - xy, 2.0 ), axis = 1))
00232 score = mag * p_set[:,2]
00233 return np.sum( score )
00234
00235
00236
00237 t0 = time.time()
00238 pos_scores = [ score_loc( i[0:2] ) for i in cm ]
00239 dt = time.time() - t0
00240
00241
00242
00243 best_ind = np.argmin( pos_scores )
00244 best_pos = cm[ best_ind ][0:2]
00245
00246
00247
00248
00249
00250
00251
00252
00253
00254
00255
00256
00257
00258
00259
00260
00261
00262
00263
00264
00265
00266
00267 theta_hats = np.linspace( -1.0 * np.pi, np.pi, 360, endpoint = False )
00268 theta_hats = np.array([ mu.standard_rad( i ) for i in theta_hats ])
00269
00270 dxy = p_set[:,0:2] - best_pos
00271 pset_thetas = np.arctan2( dxy[:,1], dxy[:,0] )
00272 pset_thetas = np.array([ mu.standard_rad( i ) for i in pset_thetas ])
00273
00274 pset_w_normed = p_set[:,2] / np.sum( p_set[:,2] )
00275
00276 def exp_err( th ):
00277 errs = np.abs([ mu.standard_rad( i ) for i in th - pset_thetas ])
00278 weighted_errs = pset_w_normed * errs
00279 mean_we = np.mean( weighted_errs )
00280 return mean_we
00281
00282 theta_hats_res = np.array([ exp_err( i ) for i in theta_hats ])
00283
00284
00285
00286 orient_est_ind = np.argmin( theta_hats_res )
00287 orient_est = theta_hats[ orient_est_ind ]
00288
00289
00290 dxy = np.linalg.norm( best_pos - loc_pos[0:2] )
00291 true_theta = np.arctan2( loc_pos[1] - best_pos[1],
00292 loc_pos[0] - best_pos[0] )
00293 dtheta = mu.standard_rad( orient_est - true_theta )
00294
00295 res = { 'loc': loc,
00296 'obj_num': obj_num,
00297 'trial_num': trial_num,
00298 'pos_readings': pos_readings,
00299 'tot_readings':tot_readings,
00300 'best_pos': best_pos,
00301 'orient_est': orient_est,
00302 'dxy': dxy,
00303 'dtheta': dtheta,
00304 'servo_yn': servo_yn,
00305 'other': { 'w_mass': w_mass,
00306 'theta_hats': theta_hats,
00307 'theta_hats_res': theta_hats_res,
00308 'p_set': p_set }}
00309
00310 return True, res, np.copy( p_set )
00311
00312
00313
00314 def MAIN_PROCESS( trial_num, obj_num, servo_yn, screen_cap = False ):
00315 print 'In MP: ', trial_num, obj_num, servo_yn, screen_cap
00316
00317 was_reads, res, p_set = process_trialobj( trial_num, obj_num, servo_yn )
00318
00319 print 'RESULTS'
00320 pprint( res )
00321
00322
00323
00324 if screen_cap:
00325 f = open( 'Obj%d_Trial%d_Servo%d_pf_results.pkl' % (obj_num, trial_num, int( servo_yn )), 'w')
00326 pkl.dump( res, f )
00327 f.close()
00328
00329
00330
00331 if not was_reads:
00332 return
00333
00334 best_pos = res[ 'best_pos' ]
00335 orient_est = res[ 'orient_est' ]
00336
00337 pub_mark = rospy.Publisher( '/tag_poses', vm.Marker )
00338
00339 if servo_yn:
00340 c_tm = [0./255, 205./255, 255./255, 1.0]
00341 else:
00342 c_tm = [255./255, 123./255, 1./255, 1.0]
00343
00344 tm = vsm( np.matrix([ best_pos[0],best_pos[1], 0.0 ]).T,
00345 np.matrix(tft.quaternion_from_euler( 0.0, 0.0, orient_est )).T,
00346 'arrow', '/map',
00347 scale = [0.5, 1.0, 1.0],
00348 color = c_tm,
00349 duration = 50.0,
00350 m_id = 2*p_set.shape[0] + 1 )
00351
00352 def pub_tm( ):
00353 tm.header.stamp = rospy.Time.now()
00354 for i in xrange( 10 ):
00355 pub_mark.publish( tm )
00356 rospy.sleep( 0.3 )
00357
00358 print 'Click on RVIZ!'
00359 time.sleep( 3 )
00360
00361 pub_tm()
00362 pub_tm()
00363 display_particles.display_trialobj( trial_num, obj_num, servo_yn, screen_cap = screen_cap )
00364 print 'Done.\n\n\n'
00365
00366 return
00367
00368
00369
00370
00371
00372
00373 if __name__ == '__main__':
00374
00375 import optparse
00376 p = optparse.OptionParser()
00377 p.add_option('--trial', action='store', type='int', dest='trial',
00378 help='trial number (0-8)')
00379 p.add_option('--obj', action='store', type='int', dest='obj',
00380 help='object number (0-8)')
00381 p.add_option('--servo', action='store_true', dest='servo',
00382 help='Use combined search and servo?', default = False)
00383 opt, args = p.parse_args()
00384
00385 obj_num = opt.obj
00386 trial_num = opt.trial
00387 servo_yn = opt.servo
00388
00389 rospy.init_node( 'goober' )
00390
00391
00392 if trial_num < 9:
00393 while not rospy.is_shutdown():
00394 print 'Publishing.'
00395 MAIN_PROCESS( trial_num, obj_num, servo_yn )
00396 else:
00397
00398
00399 for i in range( 0, 9 ):
00400 for j in range( 0, 9 ):
00401 for s in [False, True]:
00402 MAIN_PROCESS( i, j, s, screen_cap = True )