Report_AI/unitcost.py at main · Abbey721/Report_AI · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
# 均一コスト法で探索した場合の最短経路とそのコスト

class Node():

    def __init__(self, parent=None, position=None):
        self.parent = parent # 親ノードの設定
        self.position = position # (row, column)のタプル ※row：行、column：列

        self.g = 0

    def __eq__(self, other):
        # node 同士の比較演算子(==)を使用できるように
        return self.position == other.position


def astar(maze, start, end):
    # maze: 迷路リスト、start:スタートポジション、end:ゴールポジション
    # ゴールまでの最短経路のリストを返す関数

    # スタート、エンド（ゴール）ノードの初期化
    start_node = Node(None, start) # 親ノードは無し
    start_node.g = 0
    end_node = Node(None, end)
    end_node.g = 0
    cost = 0

    open_list = [] # 経路候補を入れとくリスト
    closed_list = [] # 計算終わった用済みリスト
    # Add the start node
    # 経路候補にスタートノードを追加して計算スタート
    open_list.append(start_node)

    # Loop until you find the end
    while len(open_list) > 0:

        for index, item in enumerate(open_list):
            current_node = item
            current_index = index

        # 一番小さいF値のノードをオープンリストから削除して、クローズリストに追加
        open_list.pop(current_index)
        closed_list.append(current_node)

        # ゴールに到達してれば経路(Path)を表示して終了
        if current_node == end_node:
            path = []
            current = current_node
            while current is not None:
                path.append(current.position)
                current = current.parent
            return path[::-1], cost

        # ゴールに到達してなければ子ノードを生成
        children = []
        # for new_position in [(0, -1), (0, 1), (-1, 0), (1, 0), (-1, -1), (-1, 1), (1, -1), (1, 1)]: # 斜め移動ありの場合
        for new_position in [(0, -1), (0, 1), (-1, 0), (1, 0)]: # 上下左右移動のみ

            node_position = (current_node.position[0] + new_position[0], current_node.position[1] + new_position[1])

            # 迷路内の移動に限る
            if node_position[0] > (len(maze) - 1) or node_position[0] < 0 or node_position[1] > (len(maze[len(maze)-1]) -1) or node_position[1] < 0:
                continue

            # 移動できる位置に限る（障害物は移動できない）
            if maze[node_position[0]][node_position[1]] != 0:
                continue

            # 移動できる位置のノードのみを生成
            new_node = Node(current_node, node_position)

            # 子リストに追加
            children.append(new_node)

        # 各子ノードでG, H, Fを計算
        for child in children:

            if len([closed_child for closed_child in closed_list if closed_child == child]) > 0:
                continue

            # G は今回は親ノード + 3
            child.g = current_node.g + 3

            if len([open_node for open_node in open_list if child.position == open_node.position and child.g > open_node.g]) > 0:
                continue

            # 探索コスト
            cost+=1

            # 子ノードをオープンリストに追加
            open_list.append(child)

def main():

    maze = [[0, 0, 0, 0, 0],
            [0, 1, 0, 1, 0],
            [0, 0, 0, 1, 0],
            [1, 0, 1, 1, 0],
            [0, 0, 0, 1, 0],]

    start = (0, 2)
    end = (4, 4)

    path = astar(maze, start, end)
    print(path)


if __name__ == '__main__':
    main()


"""
実行結果
([(0, 2), (0, 3), (0, 4), (1, 4), (2, 4), (3, 4), (4, 4)], 17)
"""