gridMap.removeCluster(x, y)
# FollowPath
while (time.time() - t) < 120:
#laser_point_cloud = robot.readLaser()
#laser_point_cloud = laser_point_cloud[:,:2]
#for x in range(len(laser_point_cloud)):
#mapPoints.addPoint('obstaclesLaser', *robot.localToGlobalGT(laser_point_cloud[x]))
#Convert values
rx_real = (rx[len(rx) - n - 1] - 100) / 33.33 - 4.5
ry_real = (ry[len(ry) - n - 1] - 15) / 34.64 - 7
#Show points to plot
output = robot.FollowPath(rx_real, ry_real)
#print(rx_real , ry_real)
if (output == 'true'):
print('Arrived: ', n)
n = n + 1
# GT
#x,y,orn = robot.getPosOrn()
#X.append(x)
#Y.append(y)
"""
# Encoder
x,y,orn = robot.getPosOrnOdometyEncoder()
I.append(x)
J.append(y)
