6.4.3 路径规划可视化
文件astar.py实现了一个图形化界面的路径规划应用，使用Pygame库绘制网格，并通过A和D Lite算法进行路径规划。用户可以在设计模式下设置起点、终点和障碍物，然后在执行模式下运行路径规划算法，显示路径，用户还可以在步行模式下沿规划路径行进，并在途中添加障碍物来测试路径重新规划功能。
（1）定义Pygame窗口的基本参数，包括顶部菜单栏的高度百分比、窗口的宽度和高度，以及扫描范围和Pygame窗口的初始化设置。
TOP_MENU_HEIGHT = 0.025 # 百分比
WIN_WIDTH = 800
WIN_HEIGHT = WIN_WIDTH + int((WIN_WIDTH * (TOP_MENU_HEIGHT)))
# 主网格高度 = 主网格宽度 - (主网格宽度 * (顶部菜单栏高度的百分比))
SCAN_RANGE = 1
WIN = pygame.display.set_mode((WIN_WIDTH, WIN_HEIGHT))
pygame.display.set_caption("A* Path Finding Algorithm")
pygame.font.init()（2）定义颜色常量的RGB数值，用于在Pygame中绘制不同的图形和文本时使用。
RED = (200, 0, 0) # 红色
GREEN = (0, 255, 0) # 绿色
BLUE = (0, 255, 0) # 蓝色（此处可能应为 (0, 0, 255)）
YELLOW = (255, 255, 0) # 黄色
WHITE = (255, 255, 255) # 白色
BLACK = (0, 0, 0) # 黑色
PURPLE = (128, 0, 128) # 紫色
ORANGE = (255, 165, 0) # 橙色
GREY = (128, 128, 128) # 灰色
TURQUOISE = (64, 224, 208) # 青色
BROWN = (150, 75, 0) # 棕色（3）定义类Grid，用于创建和管理二维网格。通过初始化方法可以指定网格的行数、总宽度以及起始和结束的坐标位置。方法make_grid用于生成具有指定行数的网格，每个网格包含一个Spot对象。方法reset_grid用于重新生成空的网格，方法reset_search_area用于重置网格中所有开放、关闭或路径标记的点，方法get_grid用于返回当前网格，方法get_spot用于根据行列索引或点击位置返回对应的点对象，方法draw_grid用于在Pygame窗口中绘制网格的线条。
class Grid:
def __init__(self, rows, width, start_coord, end_coord):
self.gap = width // rows # 计算每个格子的宽度
self.width = width # 网格的总宽度
self.rows = rows # 网格行数
self.start_x, self.start_y = start_coord # 起始坐标的x和y位置
self.end_x, self.end_y = end_coord # 结束坐标的x和y位置
self.grid = [] # 网格初始化为空列表
def make_grid(self):
for i in range(self.rows):
self.grid.append([]) # 为每一行创建一个空列表
for j in range(self.rows):
spot = Spot(i, j, self.gap, self.rows, # 创建网格中的每个点（Spot对象）
(self.start_x, self.start_y, self.end_x, self.end_y))
self.grid[i].append(spot) # 将创建的点添加到网格的对应位置
def reset_grid(self):
self.grid = [] # 重置网格为一个空列表
self.make_grid() # 重新生成网格
def reset_search_area(self):
for row in self.grid:
for spot in row:
if spot.is_open() or spot.is_closed() or spot.is_path():
spot.reset() # 重置网格中所有标记为开放、关闭或路径的点
def get_grid(self):
return self.grid # 返回当前网格
def get_spot(self, row=None, col=None, x=None, y=None):
if row and col:
return self.grid[row][col] # 根据行和列索引返回对应的点对象
elif x and y:
x = x - self.start_x # 转换为相对于网格起始位置的坐标
y = y - self.start_y
row = y // self.gap # 计算出点击位置在网格中的行和列索引
col = x // self.gap
print(f"spot : [{row}][{col}]") # 输出点击位置在网格中的行和列索引
return self.grid[row][col] # 返回对应的点对象
else:
pass
#TODO: add exception # TODO: 添加异常处理，暂未实现
def draw_grid(self, win):
for i in range(self.rows):
pygame.draw.line(win, GREY, (self.start_x, self.start_y + (i * self.gap)), # 在Pygame窗口中绘制网格的水平线
(self.end_x, self.start_y + (i * self.gap)))
for j in range(self.rows):
pygame.draw.line(win, GREY, (self.start_x + (j * self.gap), self.start_y), # 在Pygame窗口中绘制网格的垂直线
(self.start_x + (j * self.gap), self.end_y)))（4）定义类Rectangle，用于创建和操作矩形对象。在初始化方法中，可以指定矩形的左上角坐标、宽度和高度，默认颜色为黑色。方法draw用于在传入的Pygame窗口中绘制矩形，使用了Pygame的pygame.draw.rect函数。方法draw_text用于在窗口中绘制文本，需要传入文本内容、字体大小、颜色和位置参数，使用了Pygame的文本渲染和绘制函数。
class Rectangle:
def __init__(self, x, y, width, height):
self.x = x # 矩形左上角 x 坐标
self.y = y # 矩形左上角 y 坐标
self.width = width # 矩形宽度
self.height = height # 矩形高度
self.color = BLACK # 矩形颜色，默认为黑色
def draw(self, win):
pygame.draw.rect(win, self.color, (self.x, self.y, self.width, self.height))
# 在窗口中绘制矩形
def draw_text(self, win, text, size, color, pos):
# print(text, size, color, pos)
font = pygame.font.SysFont("Comic Sans MS", size, True)
text_surface = font.render(text, True, color)
text_rect = text_surface.get_rect(topleft=pos)
win.blit(text_surface, text_rect)
# 在窗口中绘制文本，包括文本内容、大小、颜色和位置（5）定义类TopBar，此类继承自类Rectangle，用于在Pygame窗口顶部创建一个带有模式和算法信息的条形框。在初始化方法__init__中定义了当前模式为"DESIGN"，算法名称为空字符串，以及模式文本的位置、字体大小和颜色。方法update_mode用于更新当前模式并在窗口中重新绘制模式文本，显示当前的模式信息。
class TopBar(Rectangle):
def __init__(self, x, y, width, height):
self.current_mode = "DESIGN" # 当前模式，默认为设计模式
self.alg = "" # 算法名称，默认为空字符串
self.mode_string_pos = (x+5,y+2) # 模式文本位置偏移量
# self.alg_string_pos = (x+200,y+2) # 算法文本位置偏移量（未使用）
self.mode_string_size = 10 # 模式文本字体大小
self.mode_string_color = WHITE # 模式文本颜色，默认为白色
super().__init__(x, y, width, height) # 调用父类的初始化方法，设置矩形的位置和大小
def update_mode(self, win, mode):
self.current_mode = mode # 更新当前模式为传入的模式
self.draw(win) # 调用绘制方法刷新界面
self.draw_text(win, f"MODE: {self.current_mode}", self.mode_string_size, self.mode_string_color, self.mode_string_pos)
# 绘制更新后的模式文本到窗口中（6）定义类Spot，用于表示路径规划中的单个格子或节点。在初始化方法中，它接收行索引(row)、列索引(col)、格子的宽度(width)、总行数(total_rows)、网格的起始和结束坐标范围(grid_coord)，以及可选的g值和rhs值。x和y属性被计算为网格中心点的坐标，通过给定的行列索引和格子宽度计算得出。默认情况下，颜色设置为白色，邻居节点列表初始化为空。
def __init__(self, row, col, width, total_rows, grid_coord, g=None, rhs=None):
self.row = row # 行索引
self.col = col # 列索引
self.g = g # g值（路径成本）
self.rhs = rhs # rhs值
### cant be absolute X,Y
self.x_start, self.y_start, self.x_end, self.y_end = grid_coord # 网格的起始和结束坐标范围
self.x = self.x_start + (col * width) # 计算spot的x坐标
self.y = self.y_start + (row * width) # 计算spot的y坐标
# self.x = (row * width)
# self.y = (col * width)
self.color = WHITE # 默认颜色为白色
self.neighbors = [] # 邻居节点列表
self.width = width # spot的宽度
self.total_rows = total_rows # 网格总行数（7）定义类Spot的行为，用于设置和查询单个格子的状态和属性，通过不同的颜色表示不同的状态或类型，如起点、终点、障碍物、路径等。类Spot有多个方法，包括返回该格子的行和列索引、判断格子是否处于特定状态、重置格子的状态以及将格子标记为特定状态的功能。
class Spot:
def get_pos(self):
return self.row, self.col
# 返回该spot的行和列索引
def is_closed(self):
return self.color == RED
# 判断该spot是否被标记为关闭状态（红色）
def is_open(self):
return self.color == GREEN
# 判断该spot是否被标记为开放状态（绿色）
def is_barrier(self):
return self.color == BLACK
# 判断该spot是否被标记为障碍物（黑色）
def is_object(self):
return self.color == BROWN
# 判断该spot是否被标记为物体（棕色）
def is_start(self):
return self.color == ORANGE
# 判断该spot是否被标记为起点（橙色）
def is_end(self):
return self.color == TURQUOISE
# 判断该spot是否被标记为终点（青色）
def is_path(self):
return self.color == PURPLE
# 判断该spot是否被标记为路径中的一部分（紫色）
def reset(self):
self.color = WHITE
# 重置该spot的颜色为默认的白色
def make_start(self):
self.color = ORANGE
# 将该spot标记为起点（橙色）
def make_closed(self):
self.color = RED
# 将该spot标记为关闭状态（红色）
def make_open(self):
self.color = GREEN
# 将该spot标记为开放状态（绿色）
def make_barrier(self):
self.color = BLACK
# 将该spot标记为障碍物（黑色）
def make_object(self):
self.color = BROWN
# 将该spot标记为物体（棕色）
def make_end(self):
self.color = TURQUOISE
# 将该spot标记为终点（青色）
def make_path(self):
self.color = PURPLE
# 将该spot标记为路径中的一部分（紫色）（8）定义类Spot中的update_neighbors方法，用于更新当前格子的邻居列表。通过检查当前格子上下左右四个方向相邻格子的状态，如果相邻格子不是障碍物或物体，就将其添加到当前格子的邻居列表中。这样可以帮助算法在路径搜索时遍历周围可通行的格子。
def update_neighbors(self, grid):
self.neighbors = []
if self.row < self.total_rows - 1 and not grid[self.row + 1][self.col].is_barrier() and not grid[self.row + 1][self.col].is_object(): # DOWN
self.neighbors.append(grid[self.row + 1][self.col])
if self.row > 0 and not grid[self.row - 1][self.col].is_barrier() and not grid[self.row - 1][self.col].is_object(): # UP
self.neighbors.append(grid[self.row - 1][self.col])
if self.col < self.total_rows - 1 and not grid[self.row][self.col + 1].is_barrier() and not grid[self.row][self.col + 1].is_object(): # RIGHT
self.neighbors.append(grid[self.row][self.col + 1])
if self.col > 0 and not grid[self.row][self.col - 1].is_barrier() and not grid[self.row][self.col - 1].is_object(): # LEFT
self.neighbors.append(grid[self.row][self.col - 1])（9）定义、函数reverse_path，用于将从终点到起点的路径反转为从起点到终点的形式。设置一个字典path表示路径信息，以及起点current，函数通过迭代找到从起点到终点的完整路径，并将其反转后返回。
def reverse_path(path, current):
end = current
path_list = []
reverse = {}
while current in path:
current = path[current]
path_list.append(current)
path_list.reverse()
for pos in range(len(path_list)-1):
reverse[path_list[pos]] = path_list[pos+1]
reverse[path_list[len(path_list)-1]] = end
return reverse （10）定义函数draw，用于在窗口中绘制网格和顶部菜单。函数draw根据当前模式mode和算法alg，从grid_list中获取设计网格，绘制网格中的每个格子，然后更新顶部菜单以显示当前模式和算法，最后刷新窗口以显示这些更新。
def draw(win, mode, alg, grid_list, top_menu, rows, width):
win.fill(WHITE)
# 如果当前模式是 "设计"
design_grid = grid_list[0]
grid = design_grid.get_grid()
for row in grid:
for spot in row:
spot.draw(win)
design_grid.draw_grid(win)
top_menu.update_mode(win, mode)
top_menu.update_alg(win, alg)
pygame.display.update()（11）定义函数get_clicked_spot_grid，用于确定在给定位置pos处点击的网格中的具体格子（spot）。函数遍历输入的grids列表，检查点击位置pos是否在每个网格的范围内。如果找到了包含点击位置的网格，则调用该网格的get_spot方法来获取并返回该位置的具体格子（spot）。如果没有找到匹配的网格，则返回None。
def get_clicked_spot_grid(pos, grids):
x, y = pos
clicked_grid = None
for grid in grids:
if (grid.start_x <= x <= grid.end_x) and (grid.start_y <= y <= grid.end_y):
clicked_grid = grid
break
if clicked_grid:
spot = clicked_grid.get_spot(x=x, y=y)
return spot
return None（12）实现主函数main，使用库Pygame来实现一个路径规划和执行的可视化应用。在这个项目中，用户可以通过设计模式在网格上放置起点、终点和障碍物，并选择算法来寻找从起点到终点的路径。主要功能包括创建和管理网格、响应鼠标和键盘事件来放置或清除起点、终点和障碍物，以及执行路径规划算法（A和D Lite）来计算路径并进行路径跟踪。在路径执行模式下，用户还可以通过空格键逐步移动路径或通过回车键实时执行D* Lite算法的移动和扫描过程。
def main(win, win_size, top_menu_height):
ROWS = 50
start = None
end = None
planned_path = None
current = None
k_m = 0
queue = []
g_score = {}
rhs_score = {}
mode = "DESIGN"
alg = "NONE"
width, height = win_size
start_coord_grid = (0, top_menu_height)
end_coord_grid = (width, height)
grid_width = width
design_grid = Grid(ROWS, grid_width, start_coord_grid, end_coord_grid)
design_grid.make_grid()
grids = [design_grid]
top_menu = TopBar(0, 0, width, top_menu_height)
run = True
while run:
draw(win, mode, alg, grids, top_menu, ROWS, width)
if mode == "DESIGN":
for event in pygame.event.get():
if event.type == pygame.QUIT:
run = False
if pygame.mouse.get_pressed()[0]: # 左键
pos = pygame.mouse.get_pos()
spot = get_clicked_spot_grid(pos, grids)
if not start and spot != end:
start = spot
start.make_start()
elif not end and spot != start:
end = spot
end.make_end()
elif spot != end and spot != start:
spot.make_barrier()
elif pygame.mouse.get_pressed()[2]: # 右键
pos = pygame.mouse.get_pos()
spot = get_clicked_spot_grid(pos, grids)
spot.reset()
if spot == start:
start = None
elif spot == end:
end = None
if event.type == pygame.KEYDOWN:
if event.key == pygame.K_SPACE and start and end:
upd_grid = grids[0].get_grid()
for row in upd_grid:
for spot in row:
spot.update_neighbors(upd_grid)
mode = "EXECUTION"
alg = "a-star"
if event.key == pygame.K_RETURN and start and end:
upd_grid = grids[0].get_grid()
for row in upd_grid:
for spot in row:
spot.update_neighbors(upd_grid)
mode = "EXECUTION"
alg = "d-star-lite"
if event.key == pygame.K_c:
start = None
end = None
planned_path = None
current = None
mode = "DESIGN"
grids[0].reset_grid()
if mode == "EXECUTION":
if alg == "a-star":
planned_path = a_star(lambda: draw(win, mode, alg, grids, top_menu, ROWS, width), upd_grid, start, end)
planned_path = reverse_path(planned_path, end)
print("alg execution ended! ")
mode = "WALK"
end.make_end()
current = start
elif alg == "d-star-lite":
last = start
current = start
print("running D*")
queue, k_m = d_star_lite(lambda: draw(win, mode, alg, grids, top_menu, ROWS, width), upd_grid, queue, start, end, k_m)
print("FINISHED running D*")
mode = "WALK"
start.make_start()
end.make_end()
if mode == "WALK":
for event in pygame.event.get():
if event.type == pygame.QUIT:
run = False
if pygame.mouse.get_pressed()[0]: # 左键
pos = pygame.mouse.get_pos()
spot = get_clicked_spot_grid(pos, grids)
if spot != end and spot != start and not spot.is_barrier() and not spot.is_path():
spot.make_object()
upd_grid = grids[0].get_grid()
for row in upd_grid:
for spot in row:
spot.update_neighbors(upd_grid)
elif pygame.mouse.get_pressed()[2]: # 右键
pos = pygame.mouse.get_pos()
spot = get_clicked_spot_grid(pos, grids)
if spot.is_object():
spot.reset()
if event.type == pygame.KEYDOWN:
if event.key == pygame.K_SPACE and planned_path:
next = planned_path[current]
if next != end:
if next.is_object():
start, current = current, start
start.make_start()
current.reset()
current = None
planned_path = None
grids[0].reset_search_area()
upd_grid = grids[0].get_grid()
for row in upd_grid:
for spot in row:
spot.update_neighbors(upd_grid)
mode = "EXECUTION"
else:
current = next
current.make_path()
if event.key == pygame.K_RETURN:
print(f"current position {current.get_pos()} ")
next, k_m = move_and_rescan(lambda: draw(win, mode, alg, grids, top_menu, ROWS, width), queue, current, end, SCAN_RANGE, k_m)
print(f"next position {next.get_pos()} ")
current = next
current.make_path()
if event.key == pygame.K_c:
start = None
end = None
planned_path = None
current = None
mode = "DESIGN"
grids[0].reset_grid()
pygame.quit()
main(WIN, (WIN_WIDTH, WIN_HEIGHT), int(WIN_WIDTH * (TOP_MENU_HEIGHT)))运行后使用鼠标左键点击网格来放置起点、终点和障碍物，使用鼠标右键点击网格来清除已放置的元素（起点、终点、障碍物）。按下空格键执行选择的路径规划算法（A* 或 D* Lite），按下回车键实时执行 D* Lite 算法的移动和扫描过程，按下 c 键清除所有放置的元素并回到设计模式。可视化如图6-3所示。
图6-3 执行效果
631
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
