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()
