pts = [ (0,0), (0,20), (20,20) ]
robot = Robot(window, (0,0), pts, (delta_x, delta_y))#, multi=True)
pts = [(100,210), (150,350), (250, 300)]
o = Obstacle(window, pts)
pts = [ (400, 325), (450, 325), (450, 375)]
o1 = Obstacle(window, pts)
goal = [27, 40]
cells = node.all_points(robot.loc, grid_size, grid_size)
t0 = time.clock()
# robot.plan = node.search(robot.loc, goal, dfs=True)
robot.plan = node.dijkstra(robot.loc, goal, cells, grid_size)
# heuristic = lambda x: (((x[0] - goal[0]) ** 2 + (x[1] - goal[1]) ** 2)) ** 0.5
# robot.plan = node.ucSearch(robot.loc, goal, heuristic)
print "delta", time.clock() - t0
if robot.plan is None:
print "No plan was found."
sys.exit()
drawPath(robot.plan, delta_x, delta_y)
obstacles = [o]#,o1]
safe = [robot.loc]
