def main():
# 1 生成数据
map_data = get_data(1000, 1000, 0.01)
# 2 定义起始点和目标点生成图
start_point = [86, 870]
end_point = [849, 324]
my_map = Map(map_data, start_point, end_point)
# 读取大文件数据
# 1 读取数据
number = 4650
data_dir = os.path.join(sys.path[0], 'Data')
map_data = np.load(os.path.join(data_dir, str(number) + 'm_small.npy'))
# 2 定义起点终点，然后生成图
read_position = [[500, 500, 200, 600], [1100, 460, 1150, 360],
[500, 500, 500, 2500], [2355, 2430, 2000, 4000],
[1140, 1870, 820, 3200], [1500, 20, 2355, 2430]]
# 起点终点备选
read = 0 # 规划数据，选择对那一组测试
start_position = read_position[read][:2]
end_position = read_position[read][2:]
my_map = Map(map_data, start_position, end_position)
# 3 定义算法
rrtga = RrtGa(my_map)
# 4 运行和显示结果
rrtga.find_path()
def __init__(self,
map_data,
start_position,
end_position,
num_points=200,
radius=200):
self.map = Map(map_data, start_position, end_position)
self.num_points = num_points
self.node_lists = [] # 最终有用的就是这个列表
self.cor_lists = []
self.get_nodes()
self.get_neighbors(radius)
def load_data(self, name):
'''
读取图的文件名称
'''
number = 4650
data_dir = os.path.join(sys.path[0], 'Data')
map_data = np.load(os.path.join(data_dir, str(number) + 'm_small.npy'))
self.map = Map(map_data, None, None)
with open(os.path.join(data_dir, name), 'rb') as f:
nodes = pickle.load(f)
return nodes
class WeightGraph:
'''
输入数据
数据格式:
总体是list类型,每个元素是一个Node类型
Node 包含以下因素:
x,y,neighbors(None 或者 dict 表示下个节点的索引和其对应的距离)
'''
def __init__(self, node_lists=None,data_resoure = 1):
if node_lists:
self.node_lists= node_lists
else:
self.node_lists = self.load_data('graph%d.pk'%data_resoure)
def load_data(self, name):
'''
读取图的文件名称
'''
number = 4650
data_dir = os.path.join(sys.path[0], 'Data')
map_data = np.load(os.path.join(data_dir, str(number) + 'm_small.npy'))
self.map = Map(map_data, None, None)
with open(os.path.join(data_dir, name), 'rb') as f:
nodes = pickle.load(f)
return nodes
def plot_nodes(self,lines_plot=False,paths = None,dynamic = True):
'''
画出散点图
是否画出点的连线
是否动态显示
是否输入路径
'''
self.map.plot_map(paths)
for node in self.node_lists:
plt.scatter(node.y,node.x,c= 'w',s = 10)
paths = []
if node.neighbors:
for key in node.neighbors.keys():
neigh_node = self.node_lists[key]
paths.append([node.x, node.y])
paths.append([neigh_node.x, neigh_node.y])
if lines_plot:
paths = np.array(paths)
plt.plot(paths[:, 1], paths[:, 0], c='g')
if dynamic:
plt.pause(0.0000001)
def main():
# 1 生成数据
map_data = get_data(40, 40, 0.1)
# 2 定义起始点和目标点生成图
start_point = [0, 0]
end_point = [38, 34]
my_map = Map(map_data, start_point, end_point)
'''
# # 读取大文件数据
# # 1 读取数据
# number = 4650
# data_dir = os.path.join(sys.path[0], 'Data')
# map_data = np.load(os.path.join(data_dir, str(number) + 'm_small.npy'))
# # 2 定义起点终点，然后生成图
# read_position = [[500, 500, 200, 600], [1100, 460, 1150, 360], [500, 500, 500, 2500],
# [2355, 2430, 2000, 4000], [1140, 1870, 820, 3200], [1500, 20, 2355, 2430]]
# # 起点终点备选
# read = 0 # 规划数据，选择对那一组测试
# start_position = read_position[read][: 2]
# end_position = read_position[read][2:]
# my_map = Map(map_data, start_position, end_position)
'''
# 3 定义算法
aco = Ant(my_map)
# 4 运行和显示结果
aco.find_path(100)
aco.plot_map()
aco.plot_process()
plt.show()
def main():
# # 1 生成数据
# map_data = get_data(1000, 1000, 0.01)
# # 2 定义起始点和目标点生成图
# start_point = [86, 870]
# end_point = [849, 324]
# my_map = Map(map_data, start_point, end_point)
# 读取大文件数据
# 1 读取数据
number = 4650
data_dir = os.path.join(sys.path[0], 'Data')
map_data = np.load(os.path.join(data_dir, str(number) + 'm_small.npy'))
# 2 定义起点终点，然后生成图
read_position = [[500, 500, 200, 600], [1100, 460, 1150, 360],
[500, 500, 500, 2500], [2355, 2430, 2000, 4000],
[1140, 1870, 820, 3200], [1500, 20, 2355, 2430]]
# 起点终点备选
read = 5 # 规划数据，选择对那一组测试
start_position = read_position[read][:2]
end_position = read_position[read][2:]
my_map = Map(map_data, start_position, end_position)
# 3 定义算法
time0 = time.time()
rrt = RRT(my_map)
# 4 运行和显示结果
rrt.find_path()
print(time.time() - time0)
# print(rrt.plot_final())
print(rrt.plot_process())
plt.show()
def __init__(self, width, height):
self.z = 15
self.coordinates = ['37.620070', '55.753630'] # Долгота (lon), Широта (lat)
self.pts = list()
self.type_layer = 'map'
self.org = ''
self.buttons = pygame.sprite.Group()
self.l_btn = LayersButton(self.buttons, self)
self.reset_btn = ResetButton(self.buttons, 10, 49, 'Сброс поискового результата', self)
self.search = InputField(self)
self.btn_search = SearchButton(self.buttons, self.search.outer_rect.x + 10 + self.search.outer_rect.width,
self.search.outer_rect.y, self, self.search)
self.postal_code_btn = CheckButton(self.buttons, self)
self.info = InfoField('')
self.last_search = ''
self.map = Map(self.coordinates, self.z, self.pts, self.type_layer)
self.get_map()
self.w, self.h = width, height
pygame.init()
self.screen = pygame.display.set_mode((width, height))
pygame.display.flip()
return str(x * 20000 + y)
if __name__ == '__main__':
# 1 读取数据
number = 4650
data_dir = os.path.join(sys.path[0], 'Data')
map_data = np.load(os.path.join(data_dir, str(number) + 'm_small.npy'))
# 2 定义起点终点，然后生成图
read_position = [[500, 500, 200, 600], [1100, 460, 1150, 360], [500, 500, 500, 2500],
[2355, 2430, 2000, 4000], [1140, 1870, 820, 3200], [1500, 20, 2355, 2430]]
# 起点终点备选
read = 0 # 规划数据，选择对那一组测试
start_point = read_position[read][: 2]
end_point = read_position[read][2:]
my_map = Map(map_data, start_point, end_point)
# # 与蚁群算法对比
# # 1 生成数据
# map_data = get_data(40, 40, 0.1)
# # 2 定义起始点和目标点生成图
# start_point = [0, 0]
# end_point = [38, 34]
# my_map = Map(map_data, start_point, end_point)
# 3 定义算法
model = GraphSearch(my_map, alg='B', mode=1)
# 4 运行算法，得到结果展示
time0 = time.time()
def update_map(self):
self.map = Map(self.coordinates, self.z, self.pts, self.type_layer)
self.get_map()
class MapWindow(object):
LON_STEP, LAT_STEP = 0.02, 0.008
def __init__(self, width, height):
self.z = 15
self.coordinates = ['37.620070', '55.753630'] # Долгота (lon), Широта (lat)
self.pts = list()
self.type_layer = 'map'
self.org = ''
self.buttons = pygame.sprite.Group()
self.l_btn = LayersButton(self.buttons, self)
self.reset_btn = ResetButton(self.buttons, 10, 49, 'Сброс поискового результата', self)
self.search = InputField(self)
self.btn_search = SearchButton(self.buttons, self.search.outer_rect.x + 10 + self.search.outer_rect.width,
self.search.outer_rect.y, self, self.search)
self.postal_code_btn = CheckButton(self.buttons, self)
self.info = InfoField('')
self.last_search = ''
self.map = Map(self.coordinates, self.z, self.pts, self.type_layer)
self.get_map()
self.w, self.h = width, height
pygame.init()
self.screen = pygame.display.set_mode((width, height))
pygame.display.flip()
def update_map(self):
self.map = Map(self.coordinates, self.z, self.pts, self.type_layer)
self.get_map()
def update(self):
for event in pygame.event.get():
if event.type == pygame.QUIT:
pygame.quit()
sys.exit(0)
if event.type == pygame.KEYDOWN:
if event.key == pygame.K_PAGEUP:
self.z = self.z + 1 if self.z < 19 else 19
self.update_map()
if event.key == pygame.K_PAGEDOWN:
self.z = self.z - 1 if self.z > 2 else 2
self.update_map()
if event.key == pygame.K_DOWN:
lat = self.LAT_STEP * math.pow(2, 15 - self.z)
lat = 70 + float(self.coordinates[1]) if float(self.coordinates[1]) - lat < -70 else lat
self.coordinates = self.coordinates[0], str(float(self.coordinates[1]) - lat)
self.update_map()
if event.key == pygame.K_UP:
lat = self.LAT_STEP * math.pow(2, 15 - self.z)
lat = 70 - float(self.coordinates[1]) if float(self.coordinates[1]) + lat > 70 else lat
self.coordinates = self.coordinates[0], str(float(self.coordinates[1]) + lat)
self.update_map()
if event.key == pygame.K_LEFT:
lon = self.LON_STEP * math.pow(2, 15 - self.z)
lon = 160 + float(self.coordinates[0]) if float(self.coordinates[0]) - lon < -160 else lon
self.coordinates = str(float(self.coordinates[0]) - lon), self.coordinates[1]
self.update_map()
if event.key == pygame.K_RIGHT:
lon = self.LON_STEP * math.pow(2, 15 - self.z)
lon = 160 - float(self.coordinates[0]) if float(self.coordinates[0]) + lon > 160 else lon
self.coordinates = str(float(self.coordinates[0]) + lon), self.coordinates[1]
self.update_map()
if event.type == pygame.MOUSEMOTION:
pass
if event.type == pygame.MOUSEBUTTONDOWN:
btns_array = [self.postal_code_btn.rect.collidepoint(event.pos[0], event.pos[1]),
self.l_btn.rect.collidepoint(event.pos[0], event.pos[1]),
self.search.outer_rect.collidepoint(event.pos[0], event.pos[1]),
self.reset_btn.rect.collidepoint(event.pos[0], event.pos[1]),
self.btn_search.rect.collidepoint(event.pos[0], event.pos[1])]
btns_array.extend([x.rect.collidepoint(event.pos[0], event.pos[1]) for x in self.l_btn.layers_buttons])
if not any(btns_array):
step_lon, step_lat, upper_corner_left = self.get_step()
coordinates = [str(float(upper_corner_left[0]) + step_lon * event.pos[0]),
str(float(upper_corner_left[1]) - step_lat * event.pos[1])]
if event.button == 1:
self.reset_search()
self.append_pt(coordinates[0], coordinates[1])
self.search_object(','.join(x for x in coordinates), type_of_request='click')
elif event.button == 3:
self.reset_search()
data = self.map.search_org(coordinates)
if data is not None:
self.org = data.get('name', '')
org_coordinates = data.get('coordinates')
self.append_pt(org_coordinates[0], org_coordinates[1])
self.search_object(','.join(data.get('coordinates')), type_of_request='click', org=True)
self.l_btn.update(event)
self.search.update(event)
self.btn_search.update(event)
self.reset_btn.update(event)
self.postal_code_btn.update(event)
def draw(self):
self.screen.blit(pygame.image.load(os.path.join('map_parts/', self.map.name)), (0, 0))
self.l_btn.draw(self.screen)
self.search.draw(self.screen)
self.btn_search.draw(self.screen)
self.reset_btn.draw(self.screen)
self.info.draw(self.screen)
self.postal_code_btn.draw(self.screen)
pygame.display.flip()
self.update()
def get_step(self):
lon = self.LON_STEP * math.pow(2, 15 - self.z) / 1.55
lat = self.LAT_STEP * math.pow(2, 15 - self.z) / 1.47
upper_corner_right = str(float(self.coordinates[0]) + lon), str(float(self.coordinates[1]) + lat)
lower_corner_left = str(float(self.coordinates[0]) - lon), str(float(self.coordinates[1]) - lat)
upper_corner_left = str(float(self.coordinates[0]) - lon), str(float(self.coordinates[1]) + lat)
step_lon = abs(float(lower_corner_left[0]) - float(upper_corner_right[0])) / self.w
step_lat = abs(float(lower_corner_left[1]) - float(upper_corner_right[1])) / self.h
return step_lon, step_lat, upper_corner_left
def append_pt(self, lon, lat):
self.pts.append('{},{},round'.format(lon, lat))
def reset_search(self):
self.pts.clear()
self.org = ''
self.last_search = ''
self.info.change_address('')
self.update_map()
def update_search(self):
self.search_object(self.last_search)
def search_object(self, text, type_of_request=None, org=False):
if text != '':
self.last_search = text
if type_of_request is None:
self.pts.clear()
self.search.text = ''
data = get_object_info(get_response(text))
if data is not None:
coords = data.get('coordinates')[0], data.get('coordinates')[1]
self.append_pt(coords[0], coords[1])
self.coordinates = coords
if self.postal_code_btn.state:
if org:
self.info.change_address('{}, {}. Индекс: {}'.format(data.get('address'),
self.org,
data.get('postal_code')))
else:
self.info.change_address('{}. Индекс: {}'.format(data.get('address'),
data.get('postal_code')))
else:
if org:
self.info.change_address('{}, {}'.format(data.get('address'), self.org))
else:
self.info.change_address(data.get('address'))
else:
data = get_object_info(get_response(text))
if data is not None:
self.coordinates = text.split(',')
if self.postal_code_btn.state:
if org:
self.info.change_address('{}, {}. Индекс: {}'.format(data.get('address'),
self.org,
data.get('postal_code')))
else:
self.info.change_address('{}. Индекс: {}'.format(data.get('address'),
data.get('postal_code')))
else:
if org:
self.info.change_address('{}, {}'.format(data.get('address'), self.org))
else:
self.info.change_address(data.get('address'))
self.update_map()
def get_map(self):
try:
self.map.get_map()
except BaseException as e:
print('Возникла ошибка при получении карты: {}. Работа программы завершена.'.format(e))
pygame.quit()
sys.exit(0)
class PRM:
'''
利用PRM算法将二维数据图像数据进行离散网格化
输入：二维图像，起始点，目标点， 离散点的数量，半径大小
最终一个图像可以用nodelist来表示，将二维图像变成了无向有权图
'''
def __init__(self,
map_data,
start_position,
end_position,
num_points=200,
radius=200):
self.map = Map(map_data, start_position, end_position)
self.num_points = num_points
self.node_lists = [] # 最终有用的就是这个列表
self.cor_lists = []
self.get_nodes()
self.get_neighbors(radius)
def get_neighbors(self, radius):
'''
得到点周围一定范围的neighbors
neighbors用dict来表示
'''
for index in range(self.num_points):
dis = np.linalg.norm(self.cor_lists - self.cor_lists[index],
axis=1) # 找出距离最近的点
indexs = list(np.where(dis <= radius)[0])
result = []
for index_temp in indexs:
if self.cross_obstacle(self.node_lists[index],
self.node_lists[index_temp]):
continue
if index_temp == index:
continue
result.append(index_temp)
if result:
self.node_lists[index].neighbors = {}
for res in result:
self.node_lists[index].neighbors[res] = dis[res]
def cross_obstacle(self, node1, node2):
'''
判断两节点中间的点是否经过障碍物
输入两个点的对象
'''
x1, y1 = node1.x, node1.y
x2, y2 = node2.x, node2.y
theta = math.atan2(y2 - y1, x2 - x1)
total = ((x1 - x2)**2 + (y1 - y2)**2)**0.5
for length in range(1, int(total) + 1):
newx = x1 + round(length * math.cos(theta))
newy = y1 + round(length * math.sin(theta))
if self.map.is_valid([newx, newy]) is False:
return True
return False
def get_nodes(self):
'''
生成一定数量的有效点
'''
# 加入起点
node = Node(*self.map.start)
self.node_lists.append(node)
self.cor_lists.append(self.map.start)
for _ in range(self.num_points - 2):
while True:
position = self.random_node()
if self.map.is_valid(position):
break
node = Node(*position)
self.node_lists.append(node)
self.cor_lists.append(position)
# 加入终点
node = Node(*self.map.end)
self.node_lists.append(node)
self.cor_lists.append(self.map.end)
self.cor_lists = np.array(self.cor_lists)
def random_node(self):
'''
在地图范围内生成一个随机点
'''
m, n = self.map.shape
x = random.randint(0, m)
y = random.randint(0, n)
return [x, y]
def plot(self, lines_plot=False):
'''
画出散点图
是否画出点的连线
'''
self.map.plot_map()
sns.set(style='dark')
plt.scatter(self.cor_lists[:, 1], self.cor_lists[:, 0], s=10)
if lines_plot:
for node in self.node_lists:
paths = []
if node.neighbors:
for key in node.neighbors.keys():
neigh_node = self.node_lists[key]
paths.append([node.x, node.y])
paths.append([neigh_node.x, neigh_node.y])
paths = np.array(paths)
plt.plot(paths[:, 1], paths[:, 0], c='g')
plt.pause(0.0000001)
def save(self, name):
'''
将nodelists保存到Data下的name文件里面
'''
dir = os.path.join(sys.path[0], 'Data')
with open(os.path.join(dir, name), 'wb') as f:
pickle.dump(self.node_lists, f)