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 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 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()
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()
q_rand = (randint(1, map_info.width - 1),
randint(1, map_info.height - 1))
if q_rand == map_info.start or q_rand in map_info.obstacle or rrt.is_contain(
q_rand):
continue
else:
q_rand = map_info.end
q_new = rrt.extend(q_rand, okdtree)
if not q_new:
continue
rrt.rewire(q_new, 5.0, okdtree)
if display:
map_info.set_rand(q_rand)
map_info.set_rrt(rrt.get_rrt())
# check goal
if distance(q_new, map_info.end) <= 1.0:
if q_new != map_info.end:
rrt.add(map_info.end, q_new)
return reconstruct_path(rrt, map_info.end), rrt
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_planning(m, display=True)
m.wait_close()
if y not in self._openset:
tentative_is_better = True
elif tentative_g_score < self._openset[y]['g']:
tentative_is_better = True
else:
tentative_is_better = False
if tentative_is_better:
h = self.distance(y, self._e)
self._openset[y] = {
'g': tentative_g_score,
'h': h,
'f': tentative_g_score + h,
'camefrom': x
}
if display:
self._map_info.open = y
return False
if __name__ == "__main__":
m = MapInfo(60, 40)
m.show()
m.start = (10.5, 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 ...')
plan = AStar(m.start, m.end, m)
if plan.run(display=True):
m.path = plan.reconstruct_path()
m.wait_close()
if randint(0, 10) > 3:
q_rand = (randint(1, map_info.width - 1),
randint(1, map_info.height - 1), uniform(0, math.pi * 2))
if q_rand == map_info.start or (
q_rand[0],
q_rand[1]) in map_info.obstacle or rrt.is_contain(q_rand):
continue
else:
q_rand = map_info.end
pxy = rrt.extend(q_rand, map_info)
if not pxy:
continue
rrt.rewire(q_rand, 15.0, map_info)
if display:
map_info.set_rand(q_rand)
map_info.set_rrt_dubins(rrt.get_rrt())
# check goal
if q_rand == map_info.end:
return reconstruct_path(rrt, map_info.end)
if __name__ == "__main__":
m = MapInfo(60, 40)
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)]
raw_input('enter to start ...')
m.path = rrt_star_dubins_planning(m, 2.0, display=True)
m.wait_close()
if randint(0, 10) > 2:
q_rand = (randint(1, map_info.width - 1),
randint(1, map_info.height - 1))
if q_rand == map_info.start or q_rand in map_info.obstacle or rrt.is_contain(
q_rand):
continue
else:
q_rand = map_info.end
q_new = rrt.extend(q_rand, okdtree)
if not q_new:
continue
if display:
map_info.set_rand(q_rand)
map_info.set_rrt(rrt.get_rrt())
# check goal
if distance(q_new, map_info.end) <= 1.0:
if q_new != map_info.end:
rrt.add(map_info.end, q_new)
return reconstruct_path(rrt, map_info.end)
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_planning(m, display=True)
m.wait_close()
tentative_g_score = g_score[x] + distance(x, y)
if y not in openlist:
tentative_is_better = True
elif tentative_g_score < g_score[y]:
tentative_is_better = True
else:
tentative_is_better = False
if tentative_is_better:
camefrom[y] = x
g_score[y] = tentative_g_score
openlist.append(y)
return []
def prm_planning(map_info, display=False):
samples = gen_sample_point(300, map_info)
roadmap = gen_roadmap(samples, map_info)
if display:
map_info.roadmap = roadmap
path = dijkstra_planning(roadmap, map_info)
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 = prm_planning(m, display=True)
m.wait_close()
if y not in openlist:
tentative_is_better = True
elif tentative_g_score < g_score[y]:
tentative_is_better = True
else:
tentative_is_better = False
if tentative_is_better:
if x != map_info.start and line_of_sight(y, camefrom[x], okdtree):
camefrom[y] = camefrom[x]
g_score[y] = g_score[camefrom[x]] + distance(camefrom[x], y)
h_score[y] = distance(y, map_info.end)
f_score[y] = g_score[y] + h_score[y]
else:
camefrom[y] = x
g_score[y] = tentative_g_score
h_score[y] = distance(y, map_info.end)
f_score[y] = g_score[y] + h_score[y]
openlist.append(y)
if display:
map_info.open = y
return []
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 = a_star_planning(m, display=True)
m.wait_close()