336 lines
12 KiB
Python
336 lines
12 KiB
Python
'''
|
|
coding=utf-8
|
|
Created on 2016年7月6日
|
|
|
|
@author: StarLee
|
|
'''
|
|
import tkinter as tk
|
|
from tkinter import *
|
|
from aiohttp.hdrs import LOCATION
|
|
import math
|
|
from _overlapped import NULL
|
|
import sys
|
|
class Point:
|
|
def __init__(self,x,y):
|
|
self.x,self.y = x,y
|
|
def line_dis(self,point):
|
|
return math.sqrt(math.pow(self.x-point.x,2) + math.pow(self.y-point.y,2))
|
|
def mh_dis(self,point):
|
|
return abs(self.x-point.x) + abs(self.y-point.y)
|
|
def equals(self,point):
|
|
return self.x==point.x and self.y == point.y
|
|
def next_to(self,point):
|
|
if ((abs(self.x-point.x) == 1) and self.y == point.y) or ((abs(self.y-point.y) == 1)and self.x==point.x):
|
|
return True
|
|
return False
|
|
def __str__(self):
|
|
return "%d-%d" %(self.x,self.y)
|
|
def __repr__(self):
|
|
return "%d-%d" %(self.x,self.y)
|
|
class AIPath:
|
|
|
|
|
|
def __init__(self,file_matrix,file_location):
|
|
self.file_matrix,self.file_location = file_matrix,file_location
|
|
top = tk.Tk()
|
|
top.title("Path Planning")
|
|
|
|
#划分GUI区域
|
|
self.frame_top = Frame(bg='white') #地图可视化区
|
|
#信息展示区
|
|
self.frame_center = Frame(bg='white') #地图可视化区
|
|
#信息展示区
|
|
self.frame_bottom = Frame() #交互区
|
|
self.frame_top.pack()
|
|
# self.frame_center.pack()
|
|
self.frame_bottom.pack()
|
|
|
|
|
|
self.width_canvas,self.height_canvas= 700,500
|
|
self.cvs = tk.Canvas(self.frame_top,width=self.width_canvas,height=self.height_canvas)
|
|
self.cvs.pack()
|
|
#加载地图并绘制
|
|
self.matrix = self.load_matrix(file_matrix)
|
|
self.locations = self.load_location(file_location)
|
|
self.draw_map()
|
|
|
|
|
|
#初始化控件
|
|
self.init_widget()
|
|
top.mainloop()
|
|
def load_matrix(self,file):
|
|
matrix = list()
|
|
with open(file) as fp:
|
|
for line in fp.readlines():
|
|
matrix.append([int(item) for item in line.strip().split(",")])
|
|
return matrix
|
|
|
|
def load_location(self,file):
|
|
locations = dict()
|
|
with open(file,encoding='utf-8') as fp:
|
|
for line in fp.readlines():
|
|
items = line.strip().split(",")
|
|
locations[items[0]] = [int(num) for num in items[1].split("-")]
|
|
return locations
|
|
|
|
|
|
def draw_map(self):
|
|
#矩阵和画布比例
|
|
self.row_matrix,self.column_matrix= len(self.matrix),len(self.matrix[0]) #矩阵的行列数
|
|
self.width_m,self.height_m = self.width_canvas /self.column_matrix ,self.height_canvas / self.row_matrix #矩阵的每一格宽和高
|
|
|
|
color = ['black','gray','orange','yellow','red','green','white']
|
|
|
|
for i in range(0,self.row_matrix):
|
|
for j in range(0,self.column_matrix):
|
|
# print ("%d-%d:%d" %(i,j,matrix[i][j]))
|
|
self.cvs.create_rectangle(j * self.width_m, i * self.height_m, (j+1)* self.width_m,(i+1)* self.height_m,fill = color[self.matrix[i][j]],outline = color[self.matrix[i][j]])
|
|
for key,value in self.locations.items():
|
|
self.cvs.create_text((value[0] + 0.5)* self.width_m ,(value[1] + 0.5)* self.height_m,text =key,fill="black")
|
|
|
|
# #画个坐标
|
|
# for i in range(0,self.column_matrix):
|
|
# self.cvs.create_text((i+0.5)* self.width_m,self.height_m/2,text="%d"%i,fill = "white")
|
|
# for i in range(0,self.row_matrix):
|
|
# self.cvs.create_text(self.width_m/2,(i+0.5)*self.height_m,text="%d"%i,fill = "white")
|
|
|
|
|
|
#A*算法
|
|
def a_star(self,start,end):
|
|
print("A* has been launched")
|
|
|
|
point_start = Point(self.locations[start][0],self.locations[start][1])
|
|
point_end = Point(self.locations[end][0],self.locations[end][1])
|
|
|
|
point_start.gn = 0
|
|
point_start.father = None
|
|
|
|
frontier = list()
|
|
frontier.append(point_start)
|
|
expanded = list()
|
|
|
|
while True:
|
|
print("------------new round-----------------")
|
|
if len(frontier) == 0:
|
|
print("fail")
|
|
return
|
|
point_to_ep = self.pop_frontier(frontier,point_end)
|
|
print("现在判读单节点: %d-%d" %(point_to_ep.x,point_to_ep.y))
|
|
if point_to_ep.equals(point_end) or point_to_ep.next_to(point_end):
|
|
print("succesuss")
|
|
self.get_solution(point_to_ep)
|
|
return
|
|
expanded.append(point_to_ep)
|
|
|
|
print("现在的expanded:")
|
|
print(expanded)
|
|
neighbors = self.get_neighbors(point_to_ep)
|
|
print("邻接点:")
|
|
print(neighbors)
|
|
for p in neighbors:
|
|
if self.contained(expanded, p) is False and self.contained(frontier,p) is False:
|
|
frontier.append(p)
|
|
p.father= point_to_ep
|
|
p.gn = point_to_ep.gn + 1
|
|
print("------------round ends----------------")
|
|
print("\n")
|
|
|
|
|
|
def draw_path(self,points):
|
|
length = len(points)
|
|
for i in range(0,length - 1):
|
|
self.cvs.create_line((points[i].x+0.5)*self.width_m , (points[i].y+0.5)*self.height_m,
|
|
( points[i+1].x+0.5)*self.width_m,( points[i+1].y+0.5)*self.height_m,fill = "red", arrow = tk.LAST)
|
|
|
|
#获取可行的道路邻接点
|
|
def contained(self,points,p):
|
|
for pe in points:
|
|
if(p.x == pe.x and p.y == pe.y):
|
|
return True
|
|
return False
|
|
|
|
def get_neighbors(self,point):
|
|
neighbors = list()
|
|
neighbors.append(Point(point.x-1,point.y))
|
|
neighbors.append(Point(point.x+1,point.y))
|
|
neighbors.append(Point(point.x,point.y-1))
|
|
neighbors.append(Point(point.x,point.y+1))
|
|
|
|
valid = list()
|
|
# print(self.row_matrix,self.column_matrix)
|
|
|
|
for p in neighbors:
|
|
# print(p.y,p.x,self.matrix[p.y][p.x])
|
|
if p.x<0 or p.y<0 or p.y>=self.row_matrix or p.x>=self.column_matrix or self.matrix[p.y][p.x] != 1:
|
|
continue
|
|
valid.append(p)
|
|
return valid
|
|
|
|
def get_solution(self,point):
|
|
path = list()
|
|
while point.father is not None:
|
|
path.append(point)
|
|
point = point.father
|
|
path.reverse()
|
|
|
|
######字符串输出
|
|
path_str = ''
|
|
for i in path:
|
|
path_str += "%d-%d -> " %( i.x,i.y)
|
|
print(path_str)
|
|
|
|
|
|
self.draw_path(path)
|
|
|
|
|
|
|
|
def pop_frontier(self,frontier,point_end):
|
|
min = sys.maxsize
|
|
poped = ''
|
|
pos = -1
|
|
for i in range(len(frontier)):
|
|
p = frontier[i]
|
|
fn = point_end.line_dis(p) + p.gn
|
|
if min > fn:
|
|
min = fn
|
|
poped = p
|
|
pos = i
|
|
|
|
|
|
print("挑选之前的frontier:")
|
|
print (frontier)
|
|
del p
|
|
del frontier[pos]
|
|
print("挑选之后的frontier:")
|
|
print(frontier)
|
|
|
|
return poped
|
|
|
|
def find_path(self):
|
|
|
|
self.cvs.delete('all')
|
|
self.draw_map()
|
|
|
|
#输入有效性判断
|
|
start = self.text_start.get()
|
|
end = self.text_end.get()
|
|
self.a_star(start,end)
|
|
def find_planning(self):
|
|
self.find_planning_with_nodes("top_nodes.csv")
|
|
self.find_planning_with_nodes("bottom_nodes.csv")
|
|
|
|
|
|
def load_sub_nodes(self,file):
|
|
locations = dict()
|
|
with open(file,encoding='utf-8') as fp:
|
|
for line in fp.readlines():
|
|
items = line.strip().split(",")
|
|
locations[items[0]] = ([int(num) for num in items[1].split("-")],float(items[2]))
|
|
return locations
|
|
|
|
def find_planning_with_nodes(self,file):
|
|
|
|
sub_nodes =[(Point(value[0][0],value[0][1]),value[1])for value in self.load_sub_nodes(file).values()]
|
|
|
|
point_start = Point(2,8)
|
|
point_end = Point(19,11)
|
|
|
|
point_start.gn = 0
|
|
point_start.father = None
|
|
|
|
frontier = list()
|
|
frontier.append(point_start)
|
|
expanded = list()
|
|
|
|
while True:
|
|
print("------------new round-----------------")
|
|
if len(frontier) == 0:
|
|
print("fail")
|
|
return
|
|
point_to_ep = self.pop_frontier4planning(frontier, sub_nodes)
|
|
print("现在判读单节点: %d-%d" %(point_to_ep.x,point_to_ep.y))
|
|
if point_to_ep.equals(point_end) or point_to_ep.next_to(point_end):
|
|
print("succesuss")
|
|
self.get_solution(point_to_ep)
|
|
return
|
|
expanded.append(point_to_ep)
|
|
|
|
print("现在的expanded:")
|
|
print(expanded)
|
|
neighbors = self.get_neighbors(point_to_ep)
|
|
print("邻接点:")
|
|
print(neighbors)
|
|
for p in neighbors:
|
|
if self.contained(expanded, p) is False and self.contained(frontier,p) is False:
|
|
frontier.append(p)
|
|
p.father= point_to_ep
|
|
p.gn = point_to_ep.gn + 1
|
|
print("------------round ends----------------")
|
|
print("\n")
|
|
def pop_frontier4planning(self,frontier,sub_nodes):
|
|
min = sys.maxsize
|
|
poped = ''
|
|
pos = -1
|
|
for i in range(len(frontier)):
|
|
p = frontier[i]
|
|
sum = 0
|
|
for sb in sub_nodes:
|
|
sum += sb[0].mh_dis(p)*sb[1]
|
|
fn = sum + p.gn
|
|
if min > fn:
|
|
min = fn
|
|
poped = p
|
|
pos = i
|
|
|
|
|
|
print("挑选之前的frontier:")
|
|
print (frontier)
|
|
del p
|
|
del frontier[pos]
|
|
print("挑选之后的frontier:")
|
|
print(frontier)
|
|
|
|
return poped
|
|
def init_widget(self):
|
|
|
|
#------------------fro frame center--------------------
|
|
var = StringVar()
|
|
var.set("建筑物有:")
|
|
label_locations = Label(self.frame_center,textvariable=var)
|
|
text_info = Text(self.frame_center, height=2)
|
|
label_locations.pack()
|
|
text_info.pack()
|
|
|
|
for location in self.locations.keys():
|
|
var.set("%s%s;" %(var.get(),location));
|
|
label_locations.update()
|
|
|
|
|
|
#------------------for frame bottom---------------------
|
|
##创建需要的提示/输入控件
|
|
label_start = Label(self.frame_bottom,text="起点:")
|
|
self.text_start = Entry(self.frame_bottom)
|
|
|
|
label_end = Label(self.frame_bottom,text="终点")
|
|
self.text_end = Entry(self.frame_bottom)
|
|
|
|
button_yes = Button(self.frame_bottom,width=10, text='确定',command=self.find_path)
|
|
button_planing = Button(self.frame_bottom,width=10, text='道路规划',command=self.find_planning)
|
|
|
|
#位置
|
|
label_start.grid(row = 0, column=0)
|
|
self.text_start.grid(row = 0, column=1)
|
|
label_end.grid(row = 0, column=2)
|
|
self.text_end.grid(row = 0, column=3)
|
|
button_yes.grid(row=0,column=4,padx=10)
|
|
button_planing.grid(row=0,column=5,padx=10)
|
|
#测试数据
|
|
self.text_start.insert(0,'图书馆')
|
|
self.text_end.insert(0,'17栋')
|
|
|
|
|
|
if __name__ == "__main__":
|
|
|
|
aie = AIPath("matrix.csv","location.csv")
|
|
|
|
|