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()
'''
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()
mapInfo = 0
wifiInfo = 0
# Obtains map from the server and places it on the queue
# params: queue, building name, level
def obtainMap(mapqueue, bldg, lvl):
building = bldg
level = lvl
response = urllib2.urlopen(baseurl + query)
jsondata = response.read()
orienInfo = OrientationInfo(jsondata)
mapInfo = MapInfo(jsondata)
wifiInfo = WifiInfo(jsondata)
mapqueue.put(jsondata)
# For testing:
# Run in command line using
# python map.py
if __name__ == "__main__":
response = urllib2.urlopen(baseurl + query)
jsondata = response.read()
nodes = json.loads(jsondata)
mapInfo = MapInfo(jsondata)
print mapInfo.shortestPath(15, 16)
# print "\n\n\n"
print mapInfo.shortestPathByCoordinates(450, 125, 16)
# print "\n"
print "You are off by", mapInfo.giveDirection(450, 125, 180)
# print repr(nodes)
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()
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()
# generate random point
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
dubins_path = rrt.extend(q_rand, map_info)
if not dubins_path:
continue
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_dubins_planning(m, 2.0, display=True)
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()