def main(argv=None): if (argv == None): argv = sys.argv[1:] width = 100.0 height = 100.0 pp = PathPlanningProblem(width, height, 60, 40, 40) initial, goals = pp.CreateProblemInstance() fig = plt.figure() ax = fig.add_subplot(1, 2, 1, aspect='equal') ax.set_xlim(0.0, width) ax.set_ylim(0.0, height) for o in pp.obstacles: ax.add_patch(copy.copy(o.patch)) ip = plt.Rectangle((initial[0], initial[1]), 1.0, 1.0, facecolor='#ff0000') ax.add_patch(ip) for g in goals: g = plt.Rectangle((g[0], g[1]), 1.0, 1.0, facecolor='#00ff00') ax.add_patch(g) qtd = QuadTreeDecomposition(pp, 0.2, initial, goals) qtd.Draw(ax) n = qtd.CountCells() start = timeit.default_timer() astar = AStarSearch( qtd.domain, qtd.root, Rectangle(qtd.initialCell[0], qtd.initialCell[1], 0.1, 0.1), Rectangle(qtd.goalsCell[0][0], qtd.goalsCell[0][1], 0.1, 0.1)) stop = timeit.default_timer() print("A* Running Time: ", stop - start) print("A* Path Length : ", astar.path_length) plt.plot([x for (x, y) in astar.path], [y for (x, y) in astar.path], '-') ax.set_title('Quadtree Decomposition\n{0} cells'.format(n)) ax = fig.add_subplot(1, 2, 2, aspect='equal') ax.set_xlim(0.0, width) ax.set_ylim(0.0, height) for o in pp.obstacles: ax.add_patch(copy.copy(o.patch)) ip = plt.Rectangle((initial[0], initial[1]), 1, 1, facecolor='#ff0000') ax.add_patch(ip) goal = None for g in goals: goal = g g = plt.Rectangle((g[0], g[1]), 1, 1, facecolor='#00ff00') ax.add_patch(g) start = timeit.default_timer() spath = RRT.ExploreDomain(RRT(8), pp, initial, goal, 5000) path = spath[0] final = spath[1] if len(final) == 0: print("No path found") RRT.draw(RRT(0), plt, path, final) stop = timeit.default_timer() print("RRT Running Time: ", stop - start) if len(final) > 0: print("RRT Path Length : ", RRT.pathLen(RRT(0), final)) ax.set_title('RRT') plt.show()