robot = Robot()
mapPoints = Map()
t = time.time()
Y = []
X = []
while (time.time() - t) < 60:
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]))
#Show points to plot
output = robot.GoToGoal(5, -0.85) # Insert goal X,Y
Y.append(output[0])
X.append(time.time() - t)
#Just control robot
#robot.followWallPID(True)
time.sleep(0.01)
robot.computeOdometryEncoder()
mapPoints.addPoint('robotPathGT', *robot.getPosOrn()[:2])
mapPoints.addPoint('robotPathEncoder',
*robot.getPosOrnOdometyEncoder()[:2])
#%% Obstacle fuzzy test
