def lidar_scan():
global myrobot
global p
global s
try:
#print 'ITERATION:.......', iteration
# Move robot; noise is in prob function
rel_motion = myrobot.r_motion()
print rel_motion
myrobot = myrobot.move(rel_motion)
#print 'Robot after movement: ', myrobot
# Move particles
p2 = [p[i].move(rel_motion) for i in xrange(N)]
p = p2
s.send('GE0000108000\r')
data_lidar = s.recv(BUFFER_SIZE)
# Lidar sense - returns distance to 3 beacons
lidar = ttest.update_di(data_lidar)
# Calculate the weights
w =[p[i].weight(lidar) for i in xrange(N)]
w = np.asarray(w)
w /= w.sum()
try:
# Probability random pick - use np.random alg
p3 = np.random.choice(p, N, p = w)
p = list(p3)
mean_val = [(p[i].x, p[i].y, p[i].orientation) for i in xrange(len(p))]
# Set myrobot to particle with max w
center = np.mean(mean_val, axis = 0)
myrobot.x, myrobot.y, myrobot.orientation = center[0], center[1], center[2]
except:
print 'error with choice'
pass
print myrobot
except:
traceback.print_exc()
s.send('QT\r')
s.shutdown(2)
s.close()
# Move robot; noise is in prob function
#rel_motion = myrobot.r_motion()
#print rel_motion
#myrobot = myrobot.move(rel_motion)
#print 'Robot after movement: ', myrobot
# Move particles
p2 = [p[i].move(rel_motion) for i in xrange(N)]
p = p2
#for i in p:
# print 'Particels after movement: ', i
s.send('GE0000108000\r')
data_lidar = s.recv(BUFFER_SIZE)
# Lidar sense - returns distance to 3 beacons
lidar, langle, lgraph = ttest.update_di(data_lidar)
#print 'After lidar'
#try:
# update_polar_plot(langle, lgraph)
#except:
# print 'not same length'
# Calculate the weights
#print 'lidar data: ', lidar
w = [p[i].weight(lidar) for i in xrange(N)]
w = np.asarray(w)
w /= w.sum()
#print 'just weights: ', w
#print 'sum of weights: ', np.sum(w)
try:
# Probability random pick - use np.random alg
p3 = np.random.choice(p, N, p=w)
# Move robot; noise is in prob function
#rel_motion = myrobot.r_motion()
#print rel_motion
#myrobot = myrobot.move(rel_motion)
#print 'Robot after movement: ', myrobot
# Move particles
p2 = [p[i].move(rel_motion) for i in xrange(N)]
p = p2
#for i in p:
# print 'Particels after movement: ', i
s.send('GE0000108000\r')
data_lidar = s.recv(BUFFER_SIZE)
# Lidar sense - returns distance to 3 beacons
lidar, langle, lgraph = ttest.update_di(data_lidar)
#print 'After lidar'
#try:
# update_polar_plot(langle, lgraph)
#except:
# print 'not same length'
# Calculate the weights
#print 'lidar data: ', lidar
w =[p[i].weight(lidar) for i in xrange(N)]
w = np.asarray(w)
w /= w.sum()
#print 'just weights: ', w
#print 'sum of weights: ', np.sum(w)
try:
# Probability random pick - use np.random alg
p3 = np.random.choice(p, N, p = w)
