def main3():
m = MapInfo(60, 25)
car = Car(10.0, 5.0)
start = (10, 15, 0)
end = (35, 5, 0)
m.show()
m.start = start
m.end = end
ob = [(30, i) for i in range(10)] + [(45, i) for i in range(10)] + [
(i, 10) for i in range(31)
] + [(i + 45, 10) for i in range(15)]
m.obstacle = ob
car.set_position(m.start)
car.show()
m.update()
raw_input('enter to start ...')
plan = HybridAStar(m.start, m.end, m, car, r=5.0)
if plan.run(True):
xs, ys, yaws = plan.reconstruct_path()
m.path = zip(xs, ys)
m.update()
for i in range(len(xs)):
if i != len(xs) - 1 and i % 10 != 0:
continue
plt.cla()
m.show()
m.start = start
m.end = end
m.obstacle = ob
m.path = zip(xs, ys)
car.set_position([xs[i], ys[i], yaws[i]])
car.show()
plt.pause(0.1)
m.wait_close()
def main1():
m = MapInfo(60, 40)
car = Car(10.0, 5.0)
m.show()
m.start = (10, 10, math.pi / 2)
m.end = (50, 30, math.pi / 2)
car.set_position(m.start)
car.show()
m.obstacle = [(20, i) for i in range(30)] + [(40, 40 - i)
for i in range(30)]
m.update()
raw_input('enter to start ...')
plan = HybridAStar(m.start, m.end, m, car, r=5.0)
if plan.run(True):
xs, ys, yaws = plan.reconstruct_path()
m.path = zip(xs, ys)
for i in range(len(xs)):
if i != len(xs) - 1 and i % 10 != 0:
continue
plt.cla()
m.show()
m.start = (10, 10, math.pi / 2)
m.end = (50, 30, math.pi / 2)
m.obstacle = [(20, i) for i in range(30)] + [(40, 40 - i)
for i in range(30)]
m.path = zip(xs, ys)
car.set_position([xs[i], ys[i], yaws[i]])
car.show()
plt.pause(0.1)
m.wait_close()
def main2():
m = MapInfo(600, 400)
car = Car(10.0, 5.0)
start = (10, 25, 0)
end = (450, 50, math.pi / 2)
m.show()
m.start = start
m.end = end
ob = [(40, i)
for i in range(15)] + [(50, i)
for i in range(15)] + [(i, 15)
for i in range(40)]
m.obstacle = ob
car.set_position(m.start)
car.show()
m.update()
input('enter to start ...')
plan = HybridAStar(m.start, m.end, m, car, r=5.0)
if plan.run(True):
xs, ys, yaws = plan.reconstruct_path()
m.path = list(zip(xs, ys))
m.update()
for i in range(len(xs)):
if i != len(xs) - 1 and i % 10 != 0:
continue
plt.cla()
m.show()
m.start = start
m.end = end
m.obstacle = ob
m.path = list(zip(xs, ys))
car.set_position([xs[i], ys[i], yaws[i]])
car.show()
plt.pause(0.1)
m.wait_close()
def main1():
m = MapInfo(200, 800)
car = Car(50, 20)
m.show()
m.start = (100, 10, np.pi / 2)
m.end = (100, 500, np.pi / 2)
car.set_position(m.start)
car.show()
m.obstacle = [(20, i) for i in range(15)] + [(35, 30 - i)
for i in range(15)]
m.update()
#input('enter to start ...')
plan = HybridAStar(m.start, m.end, m, car, r=10.0)
if plan.run(False):
xs, ys, yaws = plan.reconstruct_path()
m.path = list(zip(xs, ys))
for i in range(len(xs)):
if i != len(xs) - 1 and i % 10 != 0:
continue
plt.cla()
m.show()
#m.start = (10, 10, math.pi / 2)
#m.end = (50, 30, math.pi / 2)
#m.obstacle = [(20, i) for i in range(15)] + [(35, 30 - i) for i in range(15)]
m.path = list(zip(xs, ys))
car.set_position([xs[i], ys[i], yaws[i]])
car.show()
print(car._outline_x, car._outline_y)
plt.pause(0.1)
m.wait_close()
def main3():
m = MapInfo(60, 40)
car = Car(10.0, 5.0)
start = (10, 20, 0)
end = (35, 5, 0)
m.show()
m.start = start
m.end = end
ob = [(30, i) for i in range(10)] + [(45, i) for i in range(10)] + [(i, 10) for i in range(31)] + [(i+45, 10) for i in range(15)]
m.obstacle = ob
car.set_position(m.start)
car.show()
m.update()
#input('enter to start ...')
plan = HybridAStar(m.start, m.end, m, car, r=5.0)
plan.run(False)
xs,ys,yaws = plan.reconstruct_path()
'''
path, rrt = rrt_star_planning(map_info, display)
if display:
map_info.path = path
okdtree = cKDTree(map_info.obstacle)
path = path_optimization(path, rrt, okdtree, map_info, display)
for _ in range(100):
q_rand = sample(path[1:-1], okdtree, 5)
q_new = rrt.extend(q_rand, okdtree)
if not q_new:
continue
rrt.rewire(q_new, 5.0, okdtree)
path = reconstruct_path(rrt, map_info.end)
path = path_optimization(path, rrt, okdtree, map_info, display)
if display:
map_info.set_rand(q_rand)
map_info.set_rrt(rrt.get_rrt())
map_info.path = path
path = path_optimization(path, rrt, okdtree, map_info, display)
return path
if __name__ == "__main__":
m = MapInfo(60, 40)
m.show()
m.start = (10, 10)
m.end = (50, 30)
m.obstacle = [(20, i) for i in range(30)] + [(40, 40 - i)
for i in range(30)]
raw_input('enter to start ...')
m.path = rrt_star_smart_planning(m, display=True)
m.wait_close()