咸宁市网站建设_网站建设公司_代码压缩_seo优化

8.2.3 路径规划可视化

文件astar.py实现了一个基于A*算法和D* Lite算法的路径规划程序。程序提供了一个可视化界面，允许用户在网格上绘制起始点、目标点和障碍物，然后根据用户选择的算法（A或D Lite）寻找最短路径。用户可以在设计模式下绘制网格，并在执行模式下执行路径搜索和路径跟踪。程序还提供了对象放置功能，用户可以在路径搜索的过程中动态放置对象来模拟实际环境中的障碍物。

（1）下面的代码定义了程序的窗口大小、颜色常量和其他参数。其中，窗口的高度根据给定的宽度和顶部菜单栏的高度进行计算，颜色常量用于绘制网格和路径搜索过程中的不同元素。

TOP_MENU_HEIGHT = 0.025 WIN_WIDTH = 800 WIN_HEIGHT = WIN_WIDTH + int((WIN_WIDTH * (TOP_MENU_HEIGHT))) # MAIN_GRID_HEIGHT = MAIN_GRID_WIDTH - (MAIN_GRID_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() RED = (200, 0, 0) GREEN = (0, 255, 0) BLUE = (0, 255, 0) 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)

（2）下面的代码定义了类Grid，用于创建和管理网格。类Grid中包含如下所示的成员方法：

1. __init__方法用于初始化网格对象，并接受行数、宽度、起始坐标和结束坐标作为参数。
2. make_grid方法用于创建网格，通过迭代行和列来实现。
3. reset_grid方法用于重置网格，将其清空并重新创建。
4. reset_search_area方法用于重置搜索区域，将网格中所有状态为“打开”“关闭”或“路径”的点恢复为初始状态。
5. get_grid方法用于返回网格列表。
6. get_spot方法用于根据给定的行列索引或坐标返回网格中的点。
7. draw_grid方法用于绘制网格线条，通过迭代行和列来实现。

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 self.end_x, self.end_y = end_coord 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, (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 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)), (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), (self.start_x + (j * self.gap), self.end_y))

（3）定义类Rectangle，用于创建和管理程序中的矩形对象，并在图形界面中绘制矩形和文本。该类具有以下功能：

1. __init__方法用于初始化矩形对象，并接受矩形的起始坐标、宽度和高度作为参数。
2. draw方法用于绘制矩形，通过调用pygame.draw.rect函数来实现。
3. draw_text方法用于绘制文本，通过调用pygame.font.SysFont函数创建字体对象，然后调用render方法将文本渲染到表面上，并使用blit方法将文本绘制到窗口上。

class Rectangle: def __init__(self, x, y, width, height): self.x = x self.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)

（4）定义类TopBar的，它继承自类Rectangle，用于创建和管理程序中的顶部菜单栏对象，并在图形界面中绘制相应的菜单信息。该类具有以下功能：

1. __init__方法初始化顶部菜单栏对象，并接受菜单栏的起始坐标、宽度和高度作为参数。还初始化了当前模式、算法、模式字符串的位置、字符串大小和颜色等属性。
2. 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)

（5）定义类Spot，用于创建和管理程序中的路径规划格子，并在图形界面中绘制相应的格子信息。该类具有以下功能：

1. __init__方法用于初始化格子对象，接受行、列、宽度、总行数、网格坐标、g值和rhs值作为参数。根据给定的行和列计算格子的位置，并初始化颜色、邻居列表等属性。
2. get_pos方法用于返回格子的行列坐标。
3. is_closed、is_open、is_barrier、is_object、is_start、is_end、is_path方法用于检查格子的状态。
4. reset方法将格子的颜色重置为白色。
5. make_start、make_closed、make_open、make_barrier、make_object、make_end、make_path方法用于将格子设置为起点、封闭、开放、障碍、对象、终点或路径。
6. draw方法用于在窗口上绘制格子。
7. update_neighbors方法用于根据给定的网格更新格子的邻居列表。
8. __lt__方法用于比较格子对象的优先级，这里简单地返回False。

class Spot: def __init__(self, row, col, width, total_rows, grid_coord, g=None, rhs=None): self.row = row self.col = col self.g = g self.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) self.y = self.y_start + (row * width) # self.x = (row * width) # self.y = (col * width) self.color = WHITE self.neighbors = [] self.width = width self.total_rows = total_rows def get_pos(self): return self.row, self.col def is_closed(self): return self.color == RED def is_open(self): return self.color == GREEN def is_barrier(self): return self.color == BLACK def is_object(self): return self.color == BROWN def is_start(self): return self.color == ORANGE def is_end(self): return self.color == TURQUOISE def is_path(self): return self.color == PURPLE def reset(self): self.color = WHITE def make_start(self): self.color = ORANGE def make_closed(self): self.color = RED def make_open(self): self.color = GREEN def make_barrier(self): self.color = BLACK def make_object(self): self.color = BROWN def make_end(self): self.color = TURQUOISE def make_path(self): self.color = PURPLE def draw(self, win): pygame.draw.rect(win, self.color, (self.x, self.y, self.width, self.width)) 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]) def __lt__(self, other): return False

（5）函数reverse_path用于在路径规划中获取从终点到起点的路径字典，以便进行反向查找路径。首先，将当前节点设置为路径的终点，然后从当前节点开始，沿着路径逐步向前遍历，将沿途经过的节点依次添加到一个列表中。然后，将列表中的节点顺序反转，构建反转后的路径列表。接着，通过遍历反转后的路径列表，将每个节点及其后一个节点的关系添加到一个字典中。最后，将路径的最后一个节点与当前节点相连，并返回反转后的路径字典。

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

（6）函数 draw的功能是，在 Pygame 窗口中绘制网格、顶部菜单以及更新显示内容。首先用白色填充整个窗口，然后根据当前模式绘制网格和顶部菜单，并更新窗口的显示内容。

1. win：Pygame 窗口对象，用于绘制图形。
2. mode：表示当前的模式，可以是 "DESIGN" 或 "EXECUTION"。
3. alg：表示当前使用的路径规划算法。
4. grid_list：包含一个或多个网格对象的列表，用于绘制网格。
5. top_menu：顶部菜单对象，用于在窗口顶部显示当前模式和算法。
6. rows：网格的行数。
7. width：窗口的宽度。

def draw(win, mode, alg, grid_list, top_menu, rows, width): win.fill(WHITE) # if mode == "DESIGN": 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()

（7）函数get_clicked_spot_grid的功能是，根据点击位置获取所在网格的对应的网格单元（Spot）对象。

1. pos：包含点击位置的元组 (x, y)。
2. grids：包含一个或多个网格对象的列表。

函数get_clicked_spot_grid首先将点击位置的坐标解包为 x 和 y，然后遍历 grids 列表中的每个网格对象，检查点击位置是否在网格范围内。如果找到了所在的网格，则调用该网格对象的 get_spot 方法，根据点击位置的坐标获取对应的网格单元对象（Spot）。最后，返回获取到的网格单元对象。

def get_clicked_spot_grid(pos, grids): x, y = pos clicked_grid = None for grid in grids: print(x,y) print(grid.start_x, grid.end_x, grid.start_y, grid.end_y) if (grid.start_x <= x <= grid.end_x) and (grid.start_y <= y <= grid.end_y): print("grid found!" ) clicked_grid = grid break if clicked_grid: spot = clicked_grid.get_spot(x=x, y=y) return spot

（8）下面这段代码实现了一个交互式路径规划程序，用户可以通过 GUI 界面设置起点、终点和障碍物，并选择执行 A* 或 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 # in design mode start with only one grid start_coord_grid = (0, top_menu_height) end_coord_grid = (width, height) grid_width = width # Make design grid design_grid = Grid(ROWS, grid_width, start_coord_grid, end_coord_grid) design_grid.make_grid() grids = [design_grid] top_menu = 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": # run = False 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() # grid = make_grid(ROWS, width) 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": #D 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]: # LEFT 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]: # RIGHT 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(): #A *算法的行为 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)))

上述代码的实现流程如下所示：

1. 创建一个 GUI 界面，其中包含顶部菜单栏、网格区域和状态显示区域。
2. 用户可以在网格区域中设计障碍物、起点和终点，通过点击鼠标左键设置障碍物、起点和终点，通过点击鼠标右键取消障碍物或者取消起点和终点。
3. 用户可以选择执行不同的路径规划算法，包括 A*算法和 D* Lite 算法。
4. 在执行路径规划算法后，程序会展示规划的路径，并提供步进执行和连续执行两种模式。在步进执行模式下，用户可以逐步移动代理，并观察路径规划的实时结果。在连续执行模式下，程序会自动执行路径规划，并展示最终结果。
5. 用户可以在路径规划过程中对特定位置进行障碍物设置，并在路径规划完成后自动重新规划路径。
6. 用户可以随时清空网格并重新设计路径规划环境。
7. 当程序执行完成或者用户关闭窗口时，会退出程序。

运行文件astar.py会启动pygame可视化界面，可以使用鼠标进行以下操作：

1. 左键单击：放置起点、终点和障碍物。
2. 右键单击：取消放置的起点、终点或障碍物。
3. 空格键：启动路径规划算法（A或D Lite）。
4. 回车键：执行路径规划过程中的下一步操作（仅限D* Lite算法）。
5. 'c'键：清除界面上的所有内容，重新开始设计路径。

例如绘制的路径规划图效果如图8-2所示。

+

图8-2 绘制的路径规划图