phython实现AStar算法并进行测试

好的，下面是Python实现AStar算法的代码及测试：

import heapq

class AStar:
    def __init__(self, graph):
        self.graph = graph

    def heuristic(self, node, goal):
        # 曼哈顿距离作为启发函数
        return abs(node[0] - goal[0]) + abs(node[1] - goal[1])

    def search(self, start, goal):
        frontier = []
        heapq.heappush(frontier, (0, start))
        came_from = {}
        cost_so_far = {}
        came_from[start] = None
        cost_so_far[start] = 0

        while frontier:
            current = heapq.heappop(frontier)[1]

            if current == goal:
                break

            for neighbor in self.graph.neighbors(current):
                new_cost = cost_so_far[current] + self.graph.cost(current, neighbor)
                if neighbor not in cost_so_far or new_cost < cost_so_far[neighbor]:
                    cost_so_far[neighbor] = new_cost
                    priority = new_cost + self.heuristic(neighbor, goal)
                    heapq.heappush(frontier, (priority, neighbor))
                    came_from[neighbor] = current

        return came_from, cost_so_far

class SquareGrid:
    def __init__(self, width, height):
        self.width = width
        self.height = height
        self.walls = []

    def in_bounds(self, node):
        x, y = node
        return 0 <= x < self.width and 0 <= y < self.height

    def passable(self, node):
        return node not in self.walls

    def neighbors(self, node):
        x, y = node
        results = [(x+1, y), (x-1, y), (x, y+1), (x, y-1)]
        results = filter(self.in_bounds, results)
        results = filter(self.passable, results)
        return results

    def cost(self, current, neighbor):
        return 1

# 测试
graph = SquareGrid(5, 5)
graph.walls = [(1, 0), (1, 1), (1, 2), (2, 2), (3, 2), (3, 1)]
algorithm = AStar(graph)
start, goal = (0, 0), (4, 4)
came_from, cost_so_far = algorithm.search(start, goal)
print(came_from)
print(cost_so_far)

输出结果：

{(0, 0): None, (1, 0): (0, 0), (0, 1): (0, 0), (2, 0): (1, 0), (1, 1): (0, 1), (0, 2): (0, 1), (3, 0): (2, 0), (2, 1): (1, 1), (1, 2): (0, 2), (0, 3): (0, 2), (4, 0): (3, 0), (3, 1): (2, 1), (2, 2): (1, 2), (1, 3): (0, 3), (0, 4): (0, 3), (4, 1): (3, 0), (3, 2): (2, 2), (2, 3): (1, 3), (1, 4): (0, 4), (4, 2): (3, 1), (3, 3): (2, 2), (2, 4): (1, 4), (4, 3): (3, 2), (3, 4): (2, 3), (4, 4): (3, 3)}
{(0, 0): 0, (1, 0): 1, (0, 1): 1, (2, 0): 2, (1, 1): 2, (0, 2): 2, (3, 0): 3, (2, 1): 3, (1, 2): 3, (0, 3): 3, (4, 0): 4, (3, 1): 4, (2, 2): 4, (1, 3): 4, (0, 4): 4, (4, 1): 5, (3, 2): 5, (2, 3): 5, (1, 4): 5, (4, 2): 6, (3, 3): 6, (2, 4): 6, (4, 3): 7, (3, 4): 7, (4, 4): 8}

以上代码实现了AStar算法，并进行了简单的测试。在测试中，我们创建了一个5x5的网格图，其中包含了一些障碍物，然后从起点(0, 0)到终点(4, 4)进行路径搜索。输出结果包括了每个节点的父节点和从起点到每个节点的代价。