def process_trialobj( trial_num, obj_num, servo_yn ):
obj_name = tdb[obj_num][0]
tname = obj_name.replace( ' ', '' )
loc = (trial_num + obj_num) % 9
loc_name = pts[loc][0]
loc_pos = np.array(pts[loc][1]) # Tag ground-truth location
fname_prefix = '/home/travis/svn/robot1/src/projects/rfid_datacapture/src/rfid_datacapture/search_cap/'
servo_fname = fname_prefix
servo_fname += 'search_aware_home/'
servo_fname += 'woot_150_'+str(trial_num)+'_tag_'+obj_name.replace(' ','')+'_servo.pkl'
pf_search = servo_fname.replace('_servo.pkl', '_pf_search.pkl')
pf_servo = servo_fname.replace('_servo.pkl', '_pf_servo.pkl')
# Search only
search_reads_fname = fname_prefix
search_reads_fname += 'search_aware_home/woot_150_'+str(trial_num)+'_reads.pkl'
f = open( search_reads_fname, 'r' )
summary_search = pkl.load( f )
f.close()
pos_readings_search = sum([ True for p in summary_search if p.read.rssi != -1 and p.read.tagID == obj_name ])
tot_readings_search = len( summary_search )
if pos_readings_search == 0: # No results!
print '\t No results for this instance.'
res = { 'loc': loc,
'obj_num': obj_num,
'trial_num': trial_num,
'pos_readings': pos_readings_search,
'tot_readings':tot_readings_search,
'best_pos': '--',
'orient_est': '--',
'dxy': '--',
'dtheta': '--',
'servo_yn': servo_yn,
'other': { 'w_mass': 0.0, # specific to pf
'orient_fit': 0.0 }}
return False, res, None
f = open( pf_search ) # load the particle set from Search data. Will overwrite if using search plus servo
p_set_loaded = pkl.load( f )
f.close()
pos_readings = pos_readings_search
tot_readings = tot_readings_search
# If this is Search PLUS servo...
if servo_yn:
# update the stats for pos reads
f = open( servo_fname, 'r' )
summary_servo = pkl.load( f )
f.close()
pos_readings_servo = sum([ True for p in summary_servo if p.read.rssi != -1 and p.read.tagID == obj_name ])
tot_readings_servo = len( summary_servo )
pos_readings = pos_readings_search + pos_readings_servo
tot_readings = tot_readings_search + tot_readings_servo
# use the particle set from the SERVO data
f = open( pf_servo )
p_set_loaded = pkl.load( f )
f.close()
# print '\t Positive Reads: %d of %d (%2.1f)' % ( pos_readings,
# tot_readings,
# 100.0 * pos_readings / tot_readings )
# maxw = np.max( p_set_loaded[:,2] )
# minw = np.min( p_set_loaded[:,2] )
# w_normed = 1.0 * ( p_set_loaded[:,2] - minw ) / ( maxw - minw )
# # Only keep particles in the top 98% of likelihoods (250 / 255). This
# # makes the selection only consider locations where the probability
# # mass is actually located. (These are the ones displayed in the
# # screen captures!)
# p_set = p_set_loaded[ np.where( w_normed > 0.02 )[0] ]
# print 'Shape: ', p_set.shape
# # print 'p_set size (pre-cap): ', p_set.shape
# # p_set_precap = np.copy( p_set )
# # p_set = p_set[np.argsort(p_set[:,2])][:1000] # Cap the total number: keep at most the top 1000
# # print 'p_set size (pre-cap): ', p_set.shape
p_set = np.copy( p_set_loaded )
p_set = p_set[ np.argsort( p_set[:,2] )[::-1] ] # sort by decreasing weight
w_norm = p_set[:,2] / np.sum( p_set[:,2] )
w_cum = np.cumsum( w_norm )
# ONLY KEEP top 8000 particles (computation)
w_mass = w_cum[:8000][-1] * 100.0 # Ratio of mass in p_set to total:
p_set = p_set[:8000] # only keep top 8000 for computational reasons!
# # ONLY KEEP particles that are in top 98% of normalized values. (these are the ones displayed)
# maxw = np.max( p_set[:,2] )
# minw = np.min( p_set[:,2] )
# w_scaled = 1.0 * ( p_set[:,2] - minw ) / ( maxw - minw )
# p_set = p_set[ np.where( w_scaled > 0.02 )[0] ]
# p_set = p_set[:8000] # Only keep top 8000 max
# w_mass = w_cum[ p_set.shape[0] ]
# print 'p_set size (pre-cap): ', p_set.shape
# print w_mass
# print '\tShape: ', p_set.shape
pf_costmap = '/home/travis/svn/robot1/src/projects/rfid_pf/src/rfid_pf/pf_costmap.pkl'
f = open( pf_costmap )
costmap = pkl.load( f )
f.close()
cm = costmap[ np.where( costmap[:,2] )[0] ] # Locations where the robot can be located
# Determine the score for each possible xy robot location
def score_loc( xy ):
# The xy location under consideration for the robot
mag = np.sqrt( np.sum( np.power( p_set[:,0:2] - xy, 2.0 ), axis = 1)) # || dist ||
score = mag * p_set[:,2] # || dist || * w's
return np.sum( score )
# Compute all the scores for possible robot locations
t0 = time.time()
pos_scores = [ score_loc( i[0:2] ) for i in cm ]
dt = time.time() - t0
# print '\tScore computations per second: ', len( pos_scores ) * 1.0 / dt
best_ind = np.argmin( pos_scores )
best_pos = cm[ best_ind ][0:2]
# # Calculate the angle that is the mean
# # Now that we have best_pos, we need to find the best orientation.
# def score_orient( xyw, best_pos ): # returns 1x2
# # xyw is 1x3
# dxy = xyw[0:2] - best_pos # move into best_pose frame (1x2)
# dxy_unit = dxy / np.linalg.norm( dxy ) # normalize to unit circle
# return dxy_unit * xyw[2] # 1x2; [x_circ => x / |dxy| * w, y_circ...]
# so = np.array([ score_orient( i, best_pos ) for i in p_set ])
# so[ np.where(np.isnan( so )) ] = 0.0 # for positions where the particle is one and the same, the norm is 0.0 so score is nan.
# x_so = np.sum( so[:,0] )
# y_so = np.sum( so[:,1] )
# orient_est = np.arctan2( y_so, x_so )
# orient_fit = np.sqrt( x_so**2.0 + y_so**2.0 ) / ( np.sum( p_set[:,2] ))
# Brute force calculate the angle that yields the minimum |dtheta|
theta_hats = np.linspace( -1.0 * np.pi, np.pi, 360, endpoint = False )
theta_hats = np.array([ mu.standard_rad( i ) for i in theta_hats ])
dxy = p_set[:,0:2] - best_pos # put the p_set into the best_pos frame!
pset_thetas = np.arctan2( dxy[:,1], dxy[:,0] )
pset_thetas = np.array([ mu.standard_rad( i ) for i in pset_thetas ])
pset_w_normed = p_set[:,2] / np.sum( p_set[:,2] )
def exp_err( th ):
errs = np.abs([ mu.standard_rad( i ) for i in th - pset_thetas ])
weighted_errs = pset_w_normed * errs
mean_we = np.mean( weighted_errs )
return mean_we
theta_hats_res = np.array([ exp_err( i ) for i in theta_hats ])
# rrr = theta_hats
# res = theta_hats_res
orient_est_ind = np.argmin( theta_hats_res )
orient_est = theta_hats[ orient_est_ind ]
# Compute errors:
dxy = np.linalg.norm( best_pos - loc_pos[0:2] )
true_theta = np.arctan2( loc_pos[1] - best_pos[1], # y / x
loc_pos[0] - best_pos[0] )
dtheta = mu.standard_rad( orient_est - true_theta )
res = { 'loc': loc,
'obj_num': obj_num,
'trial_num': trial_num,
'pos_readings': pos_readings,
'tot_readings':tot_readings,
'best_pos': best_pos,
'orient_est': orient_est,
'dxy': dxy,
'dtheta': dtheta,
'servo_yn': servo_yn,
'other': { 'w_mass': w_mass, # specific to pf
'theta_hats': theta_hats,
'theta_hats_res': theta_hats_res,
'p_set': p_set }}
return True, res, np.copy( p_set ) # Was_reads?, results dict, particles (for display)
def process_trialobj( trial_num, obj_num, servo_yn ):
obj_name = ahe.tdb[ obj_num ][0]
tname = obj_name.replace( ' ', '' )
loc = (trial_num + obj_num) % 9
loc_name = ahe.pts[loc][0]
loc_pos = np.array(ahe.pts[loc][1]) # Tag ground-truth location
print 'Trial %d with Object %d (%s) at Position %d (%s)' % (trial_num, obj_num, obj_name, loc, loc_name)
fname = 'search_aware_home/woot_150_'+str(trial_num)+'_reads.pkl'
f = open( fname, 'r' )
summary_search = pkl.load( f )
f.close()
pos_readings_search = sum([ True for p in summary_search if p.read.rssi != -1 and p.read.tagID == obj_name ])
tot_readings_search = len( summary_search )
search_positions_fname = 'search_aware_home/woot_150_' + str(trial_num) + '_tag_' + tname + '.yaml'
servo_positions_fname = 'search_aware_home/woot_150_' + str(trial_num) + '_tag_' + tname + '_end.txt'
servo_fname = 'search_aware_home/woot_150_' + str(trial_num) + '_tag_' + tname + '_servo.pkl'
if pos_readings_search == 0:
print '\t No results for this instance.'
res = { 'loc': loc,
'obj_num': obj_num,
'trial_num': trial_num,
'pos_readings': pos_readings_search,
'tot_readings':tot_readings_search,
'best_pos': '--',
'orient_est': '--',
'dxy': '--',
'dtheta': '--',
'servo_yn': servo_yn,
'other': { 'w_mass': 0.0, # specific to pf
'orient_fit': 0.0 }}
return False, res, None
# All fnames should be defined if we got here!
f = open( search_positions_fname )
y = yaml.load( f )
f.close()
# "Best" Location determined by search
efq = tft.euler_from_quaternion
search_theta = efq( [ y['pose']['orientation']['x'],
y['pose']['orientation']['y'],
y['pose']['orientation']['z'],
y['pose']['orientation']['w'] ])[-1]
search_pos = np.array([ y['pose']['position']['x'],
y['pose']['position']['y'],
y['pose']['position']['z'] ])
search_true_theta = np.arctan2( loc_pos[1] - search_pos[1], # y / x
loc_pos[0] - search_pos[0] )
search_theta_diff = mu.standard_rad( search_theta - search_true_theta )
# search_pos_diff = np.linalg.norm( search_pos - loc_pos )
search_pos_diff = np.linalg.norm( search_pos[0:2] - loc_pos[0:2] )
# Should we return the search only results?
if not servo_yn:
res = { 'loc': loc,
'obj_num': obj_num,
'trial_num': trial_num,
'pos_readings': pos_readings_search,
'tot_readings':tot_readings_search,
'best_pos': search_pos,
'orient_est': search_theta,
'dxy': search_pos_diff,
'dtheta': search_theta_diff,
'servo_yn': servo_yn }
return True, res, None
# Else... compute the SEARCH PLUS SERVO results
# update the stats for pos reads
f = open( servo_fname, 'r' )
summary_servo = pkl.load( f )
f.close()
pos_readings_servo = sum([ True for p in summary_servo if p.read.rssi != -1 and p.read.tagID == obj_name ])
tot_readings_servo = len( summary_servo )
# Location after Servoing
f = open( servo_positions_fname )
r = f.readlines()
f.close()
# ['At time 1313069718.853\n',
# '- Translation: [2.811, 1.711, 0.051]\n',
# '- Rotation: in Quaternion [0.003, 0.001, -0.114, 0.993]\n',
# ' in RPY [0.005, 0.003, -0.229]\n',
# 'At time 1313069719.853\n',
# '- Translation: [2.811, 1.711, 0.051]\n',
# '- Rotation: in Quaternion [0.003, 0.001, -0.114, 0.993]\n',
# ' in RPY [0.005, 0.002, -0.229]\n']
rpy = r[-1].find('RPY')+3
servo_theta = json.loads( r[-1][rpy:] )[-1]
tion = r[-3].find('tion:')+5
servo_pos = np.array(json.loads( r[-3][tion:] ))
servo_true_theta = np.arctan2( loc_pos[1] - servo_pos[1], # y / x
loc_pos[0] - servo_pos[0] )
servo_theta_diff = mu.standard_rad( servo_theta - servo_true_theta )
# servo_pos_diff = np.linalg.norm( servo_pos - loc_pos )
servo_pos_diff = np.linalg.norm( servo_pos[0:2] - loc_pos[0:2] )
# print '\t Post-Servo Stats:'
# print '\t\t Tag-Robot distance err (m): %2.3f' % (servo_pos_diff)
# print '\t\t Tag-Robot orient err (deg): %2.3f' % (math.degrees(servo_theta_diff))
# print '\t\t Debug Stats', math.degrees(servo_theta), math.degrees(servo_true_theta), servo_pos, loc_pos
res = { 'loc': loc,
'obj_num': obj_num,
'trial_num': trial_num,
'pos_readings': pos_readings_search + pos_readings_servo,
'tot_readings':tot_readings_search + tot_readings_servo,
'best_pos': servo_pos,
'orient_est': servo_theta,
'dxy': servo_pos_diff,
'dtheta': servo_theta_diff,
'servo_yn': servo_yn }
return True, res, None
return
def exp_err( th ):
errs = np.abs([ mu.standard_rad( i ) for i in th - pset_thetas ])
weighted_errs = pset_w_normed * errs
mean_we = np.mean( weighted_errs )
return mean_we
def exp_err(th):
errs = np.abs([mu.standard_rad(i) for i in th - pset_thetas])
weighted_errs = pset_w_normed * errs
mean_we = np.mean(weighted_errs)
return mean_we
def process_trialobj(trial_num, obj_num, servo_yn):
obj_name = tdb[obj_num][0]
tname = obj_name.replace(' ', '')
loc = (trial_num + obj_num) % 9
loc_name = pts[loc][0]
loc_pos = np.array(pts[loc][1]) # Tag ground-truth location
fname_prefix = '/home/travis/svn/robot1/src/projects/rfid_datacapture/src/rfid_datacapture/search_cap/'
servo_fname = fname_prefix
servo_fname += 'search_aware_home/'
servo_fname += 'woot_150_' + str(trial_num) + '_tag_' + obj_name.replace(
' ', '') + '_servo.pkl'
pf_search = servo_fname.replace('_servo.pkl', '_pf_search.pkl')
pf_servo = servo_fname.replace('_servo.pkl', '_pf_servo.pkl')
# Search only
search_reads_fname = fname_prefix
search_reads_fname += 'search_aware_home/woot_150_' + str(
trial_num) + '_reads.pkl'
f = open(search_reads_fname, 'r')
summary_search = pkl.load(f)
f.close()
pos_readings_search = sum([
True for p in summary_search
if p.read.rssi != -1 and p.read.tagID == obj_name
])
tot_readings_search = len(summary_search)
if pos_readings_search == 0: # No results!
print '\t No results for this instance.'
res = {
'loc': loc,
'obj_num': obj_num,
'trial_num': trial_num,
'pos_readings': pos_readings_search,
'tot_readings': tot_readings_search,
'best_pos': '--',
'orient_est': '--',
'dxy': '--',
'dtheta': '--',
'servo_yn': servo_yn,
'other': {
'w_mass': 0.0, # specific to pf
'orient_fit': 0.0
}
}
return False, res, None
f = open(
pf_search
) # load the particle set from Search data. Will overwrite if using search plus servo
p_set_loaded = pkl.load(f)
f.close()
pos_readings = pos_readings_search
tot_readings = tot_readings_search
# If this is Search PLUS servo...
if servo_yn:
# update the stats for pos reads
f = open(servo_fname, 'r')
summary_servo = pkl.load(f)
f.close()
pos_readings_servo = sum([
True for p in summary_servo
if p.read.rssi != -1 and p.read.tagID == obj_name
])
tot_readings_servo = len(summary_servo)
pos_readings = pos_readings_search + pos_readings_servo
tot_readings = tot_readings_search + tot_readings_servo
# use the particle set from the SERVO data
f = open(pf_servo)
p_set_loaded = pkl.load(f)
f.close()
# print '\t Positive Reads: %d of %d (%2.1f)' % ( pos_readings,
# tot_readings,
# 100.0 * pos_readings / tot_readings )
# maxw = np.max( p_set_loaded[:,2] )
# minw = np.min( p_set_loaded[:,2] )
# w_normed = 1.0 * ( p_set_loaded[:,2] - minw ) / ( maxw - minw )
# # Only keep particles in the top 98% of likelihoods (250 / 255). This
# # makes the selection only consider locations where the probability
# # mass is actually located. (These are the ones displayed in the
# # screen captures!)
# p_set = p_set_loaded[ np.where( w_normed > 0.02 )[0] ]
# print 'Shape: ', p_set.shape
# # print 'p_set size (pre-cap): ', p_set.shape
# # p_set_precap = np.copy( p_set )
# # p_set = p_set[np.argsort(p_set[:,2])][:1000] # Cap the total number: keep at most the top 1000
# # print 'p_set size (pre-cap): ', p_set.shape
p_set = np.copy(p_set_loaded)
p_set = p_set[np.argsort(p_set[:, 2])[::-1]] # sort by decreasing weight
w_norm = p_set[:, 2] / np.sum(p_set[:, 2])
w_cum = np.cumsum(w_norm)
# ONLY KEEP top 8000 particles (computation)
w_mass = w_cum[:8000][-1] * 100.0 # Ratio of mass in p_set to total:
p_set = p_set[:8000] # only keep top 8000 for computational reasons!
# # ONLY KEEP particles that are in top 98% of normalized values. (these are the ones displayed)
# maxw = np.max( p_set[:,2] )
# minw = np.min( p_set[:,2] )
# w_scaled = 1.0 * ( p_set[:,2] - minw ) / ( maxw - minw )
# p_set = p_set[ np.where( w_scaled > 0.02 )[0] ]
# p_set = p_set[:8000] # Only keep top 8000 max
# w_mass = w_cum[ p_set.shape[0] ]
# print 'p_set size (pre-cap): ', p_set.shape
# print w_mass
# print '\tShape: ', p_set.shape
pf_costmap = '/home/travis/svn/robot1/src/projects/rfid_pf/src/rfid_pf/pf_costmap.pkl'
f = open(pf_costmap)
costmap = pkl.load(f)
f.close()
cm = costmap[np.where(
costmap[:, 2])[0]] # Locations where the robot can be located
# Determine the score for each possible xy robot location
def score_loc(xy):
# The xy location under consideration for the robot
mag = np.sqrt(np.sum(np.power(p_set[:, 0:2] - xy, 2.0),
axis=1)) # || dist ||
score = mag * p_set[:, 2] # || dist || * w's
return np.sum(score)
# Compute all the scores for possible robot locations
t0 = time.time()
pos_scores = [score_loc(i[0:2]) for i in cm]
dt = time.time() - t0
# print '\tScore computations per second: ', len( pos_scores ) * 1.0 / dt
best_ind = np.argmin(pos_scores)
best_pos = cm[best_ind][0:2]
# # Calculate the angle that is the mean
# # Now that we have best_pos, we need to find the best orientation.
# def score_orient( xyw, best_pos ): # returns 1x2
# # xyw is 1x3
# dxy = xyw[0:2] - best_pos # move into best_pose frame (1x2)
# dxy_unit = dxy / np.linalg.norm( dxy ) # normalize to unit circle
# return dxy_unit * xyw[2] # 1x2; [x_circ => x / |dxy| * w, y_circ...]
# so = np.array([ score_orient( i, best_pos ) for i in p_set ])
# so[ np.where(np.isnan( so )) ] = 0.0 # for positions where the particle is one and the same, the norm is 0.0 so score is nan.
# x_so = np.sum( so[:,0] )
# y_so = np.sum( so[:,1] )
# orient_est = np.arctan2( y_so, x_so )
# orient_fit = np.sqrt( x_so**2.0 + y_so**2.0 ) / ( np.sum( p_set[:,2] ))
# Brute force calculate the angle that yields the minimum |dtheta|
theta_hats = np.linspace(-1.0 * np.pi, np.pi, 360, endpoint=False)
theta_hats = np.array([mu.standard_rad(i) for i in theta_hats])
dxy = p_set[:, 0:2] - best_pos # put the p_set into the best_pos frame!
pset_thetas = np.arctan2(dxy[:, 1], dxy[:, 0])
pset_thetas = np.array([mu.standard_rad(i) for i in pset_thetas])
pset_w_normed = p_set[:, 2] / np.sum(p_set[:, 2])
def exp_err(th):
errs = np.abs([mu.standard_rad(i) for i in th - pset_thetas])
weighted_errs = pset_w_normed * errs
mean_we = np.mean(weighted_errs)
return mean_we
theta_hats_res = np.array([exp_err(i) for i in theta_hats])
# rrr = theta_hats
# res = theta_hats_res
orient_est_ind = np.argmin(theta_hats_res)
orient_est = theta_hats[orient_est_ind]
# Compute errors:
dxy = np.linalg.norm(best_pos - loc_pos[0:2])
true_theta = np.arctan2(
loc_pos[1] - best_pos[1], # y / x
loc_pos[0] - best_pos[0])
dtheta = mu.standard_rad(orient_est - true_theta)
res = {
'loc': loc,
'obj_num': obj_num,
'trial_num': trial_num,
'pos_readings': pos_readings,
'tot_readings': tot_readings,
'best_pos': best_pos,
'orient_est': orient_est,
'dxy': dxy,
'dtheta': dtheta,
'servo_yn': servo_yn,
'other': {
'w_mass': w_mass, # specific to pf
'theta_hats': theta_hats,
'theta_hats_res': theta_hats_res,
'p_set': p_set
}
}
return True, res, np.copy(
p_set) # Was_reads?, results dict, particles (for display)
def process_trialobj(trial_num, obj_num, servo_yn):
obj_name = ahe.tdb[obj_num][0]
tname = obj_name.replace(' ', '')
loc = (trial_num + obj_num) % 9
loc_name = ahe.pts[loc][0]
loc_pos = np.array(ahe.pts[loc][1]) # Tag ground-truth location
print 'Trial %d with Object %d (%s) at Position %d (%s)' % (
trial_num, obj_num, obj_name, loc, loc_name)
fname = 'search_aware_home/woot_150_' + str(trial_num) + '_reads.pkl'
f = open(fname, 'r')
summary_search = pkl.load(f)
f.close()
pos_readings_search = sum([
True for p in summary_search
if p.read.rssi != -1 and p.read.tagID == obj_name
])
tot_readings_search = len(summary_search)
search_positions_fname = 'search_aware_home/woot_150_' + str(
trial_num) + '_tag_' + tname + '.yaml'
servo_positions_fname = 'search_aware_home/woot_150_' + str(
trial_num) + '_tag_' + tname + '_end.txt'
servo_fname = 'search_aware_home/woot_150_' + str(
trial_num) + '_tag_' + tname + '_servo.pkl'
if pos_readings_search == 0:
print '\t No results for this instance.'
res = {
'loc': loc,
'obj_num': obj_num,
'trial_num': trial_num,
'pos_readings': pos_readings_search,
'tot_readings': tot_readings_search,
'best_pos': '--',
'orient_est': '--',
'dxy': '--',
'dtheta': '--',
'servo_yn': servo_yn,
'other': {
'w_mass': 0.0, # specific to pf
'orient_fit': 0.0
}
}
return False, res, None
# All fnames should be defined if we got here!
f = open(search_positions_fname)
y = yaml.load(f)
f.close()
# "Best" Location determined by search
efq = tft.euler_from_quaternion
search_theta = efq([
y['pose']['orientation']['x'], y['pose']['orientation']['y'],
y['pose']['orientation']['z'], y['pose']['orientation']['w']
])[-1]
search_pos = np.array([
y['pose']['position']['x'], y['pose']['position']['y'],
y['pose']['position']['z']
])
search_true_theta = np.arctan2(
loc_pos[1] - search_pos[1], # y / x
loc_pos[0] - search_pos[0])
search_theta_diff = mu.standard_rad(search_theta - search_true_theta)
# search_pos_diff = np.linalg.norm( search_pos - loc_pos )
search_pos_diff = np.linalg.norm(search_pos[0:2] - loc_pos[0:2])
# Should we return the search only results?
if not servo_yn:
res = {
'loc': loc,
'obj_num': obj_num,
'trial_num': trial_num,
'pos_readings': pos_readings_search,
'tot_readings': tot_readings_search,
'best_pos': search_pos,
'orient_est': search_theta,
'dxy': search_pos_diff,
'dtheta': search_theta_diff,
'servo_yn': servo_yn
}
return True, res, None
# Else... compute the SEARCH PLUS SERVO results
# update the stats for pos reads
f = open(servo_fname, 'r')
summary_servo = pkl.load(f)
f.close()
pos_readings_servo = sum([
True for p in summary_servo
if p.read.rssi != -1 and p.read.tagID == obj_name
])
tot_readings_servo = len(summary_servo)
# Location after Servoing
f = open(servo_positions_fname)
r = f.readlines()
f.close()
# ['At time 1313069718.853\n',
# '- Translation: [2.811, 1.711, 0.051]\n',
# '- Rotation: in Quaternion [0.003, 0.001, -0.114, 0.993]\n',
# ' in RPY [0.005, 0.003, -0.229]\n',
# 'At time 1313069719.853\n',
# '- Translation: [2.811, 1.711, 0.051]\n',
# '- Rotation: in Quaternion [0.003, 0.001, -0.114, 0.993]\n',
# ' in RPY [0.005, 0.002, -0.229]\n']
rpy = r[-1].find('RPY') + 3
servo_theta = json.loads(r[-1][rpy:])[-1]
tion = r[-3].find('tion:') + 5
servo_pos = np.array(json.loads(r[-3][tion:]))
servo_true_theta = np.arctan2(
loc_pos[1] - servo_pos[1], # y / x
loc_pos[0] - servo_pos[0])
servo_theta_diff = mu.standard_rad(servo_theta - servo_true_theta)
# servo_pos_diff = np.linalg.norm( servo_pos - loc_pos )
servo_pos_diff = np.linalg.norm(servo_pos[0:2] - loc_pos[0:2])
# print '\t Post-Servo Stats:'
# print '\t\t Tag-Robot distance err (m): %2.3f' % (servo_pos_diff)
# print '\t\t Tag-Robot orient err (deg): %2.3f' % (math.degrees(servo_theta_diff))
# print '\t\t Debug Stats', math.degrees(servo_theta), math.degrees(servo_true_theta), servo_pos, loc_pos
res = {
'loc': loc,
'obj_num': obj_num,
'trial_num': trial_num,
'pos_readings': pos_readings_search + pos_readings_servo,
'tot_readings': tot_readings_search + tot_readings_servo,
'best_pos': servo_pos,
'orient_est': servo_theta,
'dxy': servo_pos_diff,
'dtheta': servo_theta_diff,
'servo_yn': servo_yn
}
return True, res, None
return
f.close()
# "Best" Location determined by search
efq = tft.euler_from_quaternion
search_theta = efq( [ y['pose']['orientation']['x'],
y['pose']['orientation']['y'],
y['pose']['orientation']['z'],
y['pose']['orientation']['w'] ])[-1]
search_pos = np.array([ y['pose']['position']['x'],
y['pose']['position']['y'],
y['pose']['position']['z'] ])
search_true_theta = np.arctan2( loc_pos[1] - search_pos[1], # y / x
loc_pos[0] - search_pos[0] )
search_theta_diff = mu.standard_rad( search_theta - search_true_theta )
search_pos_diff = np.linalg.norm( search_pos - loc_pos )
print '\t Post-Search Stats:'
print '\t\t Tag-Robot distance err (m): %2.3f' % (search_pos_diff)
print '\t\t Tag-Robot orient err (deg): %2.3f' % (math.degrees(search_theta_diff))
print '\t\t Debug Stats', math.degrees(search_theta), math.degrees(search_true_theta), search_pos, loc_pos
# Location after Servoing
f = open( servo_fname )
r = f.readlines()
f.close()
# ['At time 1313069718.853\n',
# '- Translation: [2.811, 1.711, 0.051]\n',
# '- Rotation: in Quaternion [0.003, 0.001, -0.114, 0.993]\n',
# ' in RPY [0.005, 0.003, -0.229]\n',
