用Python写一段代码,随机生成20*20且有障碍物的地图,利用Astar算法寻找起点到终点路径
时间: 2024-01-22 17:17:26 浏览: 110
Python基于DWA算法和Astar算法的轮式机器人路径规划源码+使用说明.zip
好的,下面是代码实现:
```python
import random
import heapq
class Node:
def __init__(self, x, y, is_obstacle=False):
self.x = x
self.y = y
self.is_obstacle = is_obstacle
self.g = 0
self.h = 0
self.f = 0
self.parent = None
def __lt__(self, other):
return self.f < other.f
def __eq__(self, other):
return self.x == other.x and self.y == other.y
class AStar:
def __init__(self, start, goal, grid):
self.start = start
self.goal = goal
self.grid = grid
def heuristic(self, a, b):
return abs(a.x - b.x) + abs(a.y - b.y)
def get_neighbors(self, node):
neighbors = []
for dx, dy in ((-1, 0), (0, -1), (1, 0), (0, 1)):
x = node.x + dx
y = node.y + dy
if x < 0 or y < 0 or x >= len(self.grid) or y >= len(self.grid[0]):
continue
if self.grid[x][y].is_obstacle:
continue
neighbors.append(self.grid[x][y])
return neighbors
def search(self):
open_list = []
closed_list = []
heapq.heappush(open_list, self.start)
while open_list:
current = heapq.heappop(open_list)
if current == self.goal:
path = []
while current.parent:
path.append((current.x, current.y))
current = current.parent
path.append((current.x, current.y))
return path[::-1]
closed_list.append(current)
for neighbor in self.get_neighbors(current):
if neighbor in closed_list:
continue
tentative_g = current.g + 1
if neighbor not in open_list:
heapq.heappush(open_list, neighbor)
neighbor.h = self.heuristic(neighbor, self.goal)
neighbor.g = tentative_g
neighbor.f = neighbor.g + neighbor.h
neighbor.parent = current
elif tentative_g < neighbor.g:
neighbor.g = tentative_g
neighbor.f = neighbor.g + neighbor.h
neighbor.parent = current
return None
def generate_obstacles(n, m):
grid = [[Node(i, j) for j in range(m)] for i in range(n)]
for i in range(n):
for j in range(m):
if random.random() < 0.2:
grid[i][j].is_obstacle = True
return grid
def print_grid(grid, path=None):
for i in range(len(grid)):
for j in range(len(grid[0])):
if grid[i][j].is_obstacle:
print("X", end=" ")
elif path and (i, j) in path:
print("*", end=" ")
else:
print("_", end=" ")
print()
if __name__ == '__main__':
grid = generate_obstacles(20, 20)
start = grid[0][0]
goal = grid[19][19]
astar = AStar(start, goal, grid)
path = astar.search()
print_grid(grid, path)
print()
if path:
print("Path found!")
print(path)
else:
print("No path found!")
```
代码运行后会随机生成一个20x20的地图,并在其中随机放置障碍物。然后使用A*算法寻找从左上角到右下角的最短路径,并将路径打印出来。
阅读全文