幅優先探索

経路計画を勉強する第一歩として，幅優先探索について学ぶ．

流れとしては以下の通り，
1. [迷路の生成](#迷路の生成)：障害物とスタート＆ゴール地点をランダムに設定し，探索する迷路の生成する
2. [探索](#探索)：障害物を避けながら，スタート→ゴールまでの経路を探索する
3. [経路表示](#経路表示)：探索結果を探索領域および最短経路として表示する

経路表示をターミナル上に文字で行っているものが多かったのですが，やっぱりちゃんとグラフで＆アニメーションで表示された方が気分も上がりますよね！
    ![test.gif](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3250323/457dc12b-a254-1e36-ae1e-6b01157b866d.gif)


# コードの全体像
```python:main.py
import itertools
import random
import matplotlib.pyplot as plt
from matplotlib import patches
from matplotlib.animation import FuncAnimation

class Maze:
    def __init__(self, map_size=(10,10), object_num=25):
        self.map_size = map_size
        self.object_num = object_num
        self.maze = [[0] * self.map_size[0] for i in range(self.map_size[1])]
        pass

    def choice_and_del(self, choice_list, choice_num):
        ret_list = []
        for _ in range(choice_num):
            # set index number
            index = random.choice(range(len(choice_list)))
            # get the value of identified index and delete it from the choice_list
            # add the value to ret_list 
            ret_list.append(choice_list.pop(index))
        return ret_list

    def generation_map(self):
        # return coordinate list ([0,0], [0,1], [0,2] .... [5,3],[5,4],[5,5])
        ## itertools.puroduct returns puroduct(積) of 0~5 x 0~5
        map_list = list(itertools.product(range(self.map_size[0]), range(self.map_size[1])))

        for i in range(self.map_size[1]):
            self.maze[0][i] = 9
            self.maze[self.map_size[0] - 1][i] = 9

        for j in range(self.map_size[0]):
            self.maze[j][0] = 9
            self.maze[j][self.map_size[1] - 1] = 9

        # set cost of objects (like wall...). Number of object is set at first
        if self.object_num > 0:
            object_coords = self.choice_and_del(map_list, self.object_num)
            for object_coord in object_coords:
                self.maze[object_coord[0]][object_coord[1]] = 9

        # choice one coordinate as a start, which doesn't on object(cost 9)
        ## and convert it to the list style
        start = self.choice_and_del(map_list, 1)
        while self.maze[start[0][0]][start[0][1]] == 9:
            start = self.choice_and_del(map_list, 1)

        self.start = Position(start[0][0], start[0][1], 0)
        # choice one coordinate as a goal, which doesn't on object(cost 9)
        goal = self.choice_and_del(map_list, 1)
        while self.maze[goal[0][0]][goal[0][1]] == 9 or self.maze[goal[0][0]][goal[0][1]] == 9:
            goal = self.choice_and_del(map_list, 1)
        # set cost of goal
        self.maze[goal[0][0]][goal[0][1]] = 1
        self.goal = Position(goal[0][0], goal[0][1], 0)
        return self.maze, self.start

    def twice_graph(self, route_list, trace_list):
        fig = plt.figure(figsize=self.map_size)

        #グラフを描画するsubplot領域を作成。
        ax1 = fig.add_subplot(1, 2, 1)
        ax2 = fig.add_subplot(1, 2, 2)

    
        ax1.set_xticks(list(range(self.map_size[0]+1)))
        ax1.set_yticks(list(range(self.map_size[1]+1)))
        ax1.grid()
        ax1.set_aspect('equal')

        
        ax2.set_xticks(list(range(self.map_size[0]+1)))
        ax2.set_yticks(list(range(self.map_size[1]+1)))
        ax2.grid()
        ax2.set_aspect('equal')


        # for before
        for x in range(self.map_size[0]):
                for y in range(self.map_size[1]):
                    if self.maze[x][y] == 9:
                        r = patches.Rectangle( xy=(x,y), width=1, height=1, color="grey", alpha=0.5) # 四角形のオブジェクト
                        # 4. Axesに図形オブジェクト追加・表示
                        ax1.add_patch(r)

        # # show start&goal position with box
        # r = patches.Rectangle( xy=(self.start.x,self.start.y), width=1, height=1, color="blue", alpha=0.5) # 四角形のオブジェクト
        # ax1.add_patch(r)
        # r = patches.Rectangle( xy=(self.goal.x,self.goal.y), width=1, height=1, color="red", alpha=0.5) # 四角形のオブジェクト
        # ax1.add_patch(r)

        # show start&goal position with star
        s2g = [[self.start.convert_coordinate().x, self.goal.convert_coordinate().x],
                 [self.start.convert_coordinate().y, self.goal.convert_coordinate().y]]
        
        ax1.scatter(s2g[0], s2g[1], c="orange", marker="*", s=350)
        ax2.scatter(s2g[0], s2g[1], c="orange", marker="*", s=350, zorder=5)
        
        # for after
        for x in range(self.map_size[0]):
            for y in range(self.map_size[1]):
                if self.maze[x][y] == 9:
                    r = patches.Rectangle( xy=(x,y), width=1, height=1, color="grey",alpha=0.5) # 四角形のオブジェクト
                    # 4. Axesに図形オブジェクト追加・表示
                    ax2.add_patch(r)

        # show all node explored with blue x
        for i in range(len(trace_list)):
            ax2.scatter(trace_list[i].convert_coordinate().x, trace_list[i].convert_coordinate().y, c="blue", marker="x")
        
        # # show all node explored with yellow box
        # for i in range(len(trace_list)):
        #     r = patches.Rectangle( xy=(trace_list[i].x, trace_list[i].y), width=1, height=1, color="yellow", alpha=0.5) # 四角形のオブジェクト
        #     # 4. Axesに図形オブジェクト追加・表示
        #     ax2.add_patch(r)

        # show route node with red x
        for i in range(len(route_list)):
            ax2.scatter(route_list[i].convert_coordinate().x, route_list[i].convert_coordinate().y, c="red", marker="x")
        
        # # show route node with red green box
        # for i in range(len(route_list)):
        #     r = patches.Rectangle( xy=(route_list[i].x, route_list[i].y), width=1, height=1, color="green", alpha=0.5) # 四角形のオブジェクト
        #     # 4. Axesに図形オブジェクト追加・表示
        #     ax2.add_patch(r)

        # # show start&goal position with box
        # r = patches.Rectangle( xy=(self.start.x,self.start.y), width=1, height=1, color="blue", alpha=0.5) # 四角形のオブジェクト
        # ax2.add_patch(r)
        # r = patches.Rectangle( xy=(self.goal.x,self.goal.y), width=1, height=1, color="red", alpha=0.5) # 四角形のオブジェクト
        # ax2.add_patch(r)

        plt.show()
    
    def set_maze_graph(self):
        fig, ax = plt.subplots(figsize=self.map_size)
    
        ax.set_xticks(list(range(self.map_size[0]+1)))
        ax.set_yticks(list(range(self.map_size[1]+1)))
        ax.grid()
        ax.set_aspect('equal')

        # set maze
        for x in range(self.map_size[0]):
                for y in range(self.map_size[1]):
                    if self.maze[x][y] == 9:
                        r = patches.Rectangle( xy=(x,y), width=1, height=1, color="grey", alpha=0.5) # 四角形のオブジェクト
                        # 4. Axesに図形オブジェクト追加・表示
                        ax.add_patch(r)
        # set star&goal position
        s2g = [[self.start.convert_coordinate().x, self.goal.convert_coordinate().x],
                 [self.start.convert_coordinate().y, self.goal.convert_coordinate().y]]
        
        ax.scatter(s2g[0], s2g[1], c="orange", marker="*", s=350)
        return fig, ax

    def animation(self, frame):
        ax.clear()
        # draw maze
        for x in range(self.map_size[0]):
                for y in range(self.map_size[1]):
                    if self.maze[x][y] == 9:
                        r = patches.Rectangle( xy=(x,y), width=1, height=1, color="grey", alpha=0.5) # 四角形のオブジェクト
                        # 4. Axesに図形オブジェクト追加・表示
                        ax.add_patch(r)
        # set start&goal
        s2g = [[self.start.convert_coordinate().x, self.goal.convert_coordinate().x],
                 [self.start.convert_coordinate().y, self.goal.convert_coordinate().y]]
        
        ax.scatter(s2g[0], s2g[1], c="orange", marker="*", s=350)
        plt.text(self.start.x + 0.7, self.start.y + 0.7, "S")
        plt.text(self.goal.x + 0.7, self.goal.y + 0.7, "G")
        
        # trace the route
        for i in range(frame+1):
            ax.scatter(trace_list[i].convert_coordinate().x, trace_list[i].convert_coordinate().y, c="blue", marker="x", zorder=0)

        # show route list at the end
        if frame+1 == len(trace_list):
            for i in range(1,len(route_list)-1):
                ax.scatter(route_list[i].convert_coordinate().x, route_list[i].convert_coordinate().y, c="red", marker="x", zorder=1)
        
        ax.set_xlim([0, self.map_size[0]])
        ax.set_ylim([0, self.map_size[1]])
        ax.set_aspect('equal')


class Position:
    def __init__(self, x, y, depth=0, parent_x=-1, parent_y=-1):
        self.x = x                  # x座標 
        self.y = y                  # y座標
        self.depth = depth          # 移動回数
        self.parent_x = parent_x    # 一つ前に通ったマスのx座標
        self.parent_y = parent_y    # 一つ前に通ったマスのy座標

    def add_around(self):
        x, y, depth = self.x, self.y, self.depth
        left_pos = Position(x-1, y, depth+1, x, y)
        right_pos = Position(x+1, y, depth+1, x, y)
        up_pos = Position(x, y-1, depth+1, x, y)
        down_pos = Position(x, y+1, depth+1, x, y)
        
        around_list = []
        
        around_list.extend([left_pos, right_pos, up_pos, down_pos])
        return around_list
    
    def convert_coordinate(self):
        graph_coordinate = Position(self.x+0.5, self.y+0.5)
        return graph_coordinate
    
def RouteRecord(trace_list):
    # get number of all traced position to achieve the goal
    num_trace = len(trace_list)
    # num_goal = num_trace - 1
    n = num_trace - 1
    route_list = []
    cnt = 0
    while True:
        child = trace_list[n]
        route_list.append(child)
        if child.parent_x > 0:
            # find the list number of goal's parent position and update "n" to it
            for i in range(num_trace):
                p = trace_list[i]
                if p.x == child.parent_x and p.y == child.parent_y:
                        n = i
                        break
                else:
                    continue
                
        else:
            break
    return route_list


# 探索関数：ゴールしたらそのときの位置・移動数を返す
def Search(start, map_size=(10,10)):
    # スタート位置（x座標, y座標, 移動回数）をセット
    explore_list = [start]
    # set list to trace path
    trace_list = [start]
    
    # while len(explore_list) > 0:# 探索可能ならTrue(listが空ならfalse)
    while len(explore_list)>0:
        last_point = explore_list.pop(0) # リストから探索する位置を取得
        # update explore list
        around_list = last_point.add_around()
        for i in range(len(around_list)):
            p = around_list[i]
            # if the point is outside of maze or on object, skip
            if maze[p.x][p.y] == 9:
                continue
            # if the point is already on the explore list, skip
            elif maze[p.x][p.y] == 2:
                continue
            # if the point is goal, FINISH
            elif maze[p.x][p.y] == 1:                
                trace_list.append(p)
                route_list = RouteRecord(trace_list)
                return route_list, trace_list
            # if the point should be explored, add to the explore list         
            elif maze[p.x][p.y] == 0:
                maze[p.x][p.y] = 2
                # update explore list
                explore_list.append(p)
                trace_list.append(p)
                continue
            # if the point is arounded by objects&explored point, skip 
            else:
                # print("No route to go")
                continue
    return 0

if __name__ == '__main__':
    create_maze = Maze()
    maze, start = create_maze.generation_map() # generate map
    fig, ax = create_maze.set_maze_graph()
    route_list, trace_list= Search(start)  # 探索
    # アニメーション化
    anim = FuncAnimation(fig, create_maze.animation, frames=len(trace_list), interval=100, repeat=False)
    anim.save("test.gif", writer="pillow")
    plt.show()
```

## 迷路の生成
迷路の生成はMaze()クラスで行う．行列の形で任意の迷路を生成する
* map_size：迷路の大きさ．デフォルトで10x10
* object_num：障害物の数．デフォルトで25個
* コスト：迷路内の各座標にコストが割り振られている．その値によって，探索時の対応が変わる
    * 普通の通路：通行可能（障害物がない）部分．コストは「0」
    * スタート地点：普通の通路と同様，コストは「0」
    * ゴール地点：ゴール到着を判断するため，コストは「1」
    * 障害物：通行不可の領域．コストは「9」
    * 迷路の外周：探索領域が迷路外に出ないよう，外周のコストは障害物と同じ「9」

迷路生成の詳細については，☟を参照．

https://qiita.com/Karin-Sugi/items/cb40638d9033ac5ec326#%E8%BF%B7%E8%B7%AF%E6%8E%A2%E7%B4%A2%E7%94%A8%E8%BF%B7%E8%B7%AF

## 探索
[こちらのサイト](https://qiita.com/Yuya-Shimizu/items/eba62e9c6880a606736e)をもとに作成．
深さ探索では，スタート地点→スタート地点の（左右上下）周囲4点→各4点の周囲4点．．． というように，スタート地点からの移動回数（深さ）が同じ点を中心に探索領域を広げていく．

このとき，一番最初にゴールにたどり着いた経路が必ず最短経路となる．

以下のコードは，
1. スタートからゴールまで探索：*Search( )*
    ただし，障害物や外周，すでに通ったマスなどは探索領域から除外する
2. ゴールから順に一つ前で通ったマスを遡ることで，最短経路を取得：*RouteRecord( )*

以上，2つの構成からなる．

```python:main.py
# 各座標の情報を格納するクラス
class Position:
    def __init__(self, x, y, depth=0, parent_x=-1, parent_y=-1):
        self.x = x                  # x座標 
        self.y = y                  # y座標
        self.depth = depth          # 移動回数
        self.parent_x = parent_x    # 一つ前に通ったマスのx座標（スタート地点のみ-1）
        self.parent_y = parent_y    # 一つ前に通ったマスのy座標（スタート地点のみ-1）
    
    # 自身の周囲 左右上下4点を返す関数
    def add_around(self):
        x, y, depth = self.x, self.y, self.depth
        left_pos = Position(x-1, y, depth+1, x, y)
        right_pos = Position(x+1, y, depth+1, x, y)
        up_pos = Position(x, y-1, depth+1, x, y)
        down_pos = Position(x, y+1, depth+1, x, y)
        
        around_list = []
        
        around_list.extend([left_pos, right_pos, up_pos, down_pos])
        return around_list
    
# ゴールから一つ前のマスを遡ることで，ゴールとスタートを繋ぐ最短経路を表示
def RouteRecord(trace_list):
    # trace_listは探索したすべての座標の情報が，探索順に格納されている
    ## リストの先頭がスタート地点，最後尾がゴール地点
    # get number of all traced position to achieve the goal
    num_trace = len(trace_list)
    # はじめのnはゴール地点をさす．
    n = num_trace - 1
    route_list = []

    while True:
        child = trace_list[n]
        route_list.append(child)
        # child（現在の）マスがスタート地点ではないとき
        if child.parent_x > 0:
            # trace_listの中から，現在のマスの親（一つ前のマス）を探す
            # find the list number of goal's parent position and update "n" to it
            for i in range(num_trace):
                p = trace_list[i]
                if p.x == child.parent_x and p.y == child.parent_y:
                        n = i
                        break
                else:
                    continue
        # childがスタート地点のとき（スタート地点まで遡り終えたとき） 
        else:
            # 終了
            break
    return route_list


# 探索関数：ゴールするまで，探索を続ける
def Search(start, map_size=(10,10)):
    # スタート位置（x座標, y座標, 移動回数）をセット
    explore_list = [start]
    # set list to trace path
    trace_list = [start]
    
    # while len(explore_list) > 0:# 探索可能ならTrue(listが空ならfalse)
    while len(explore_list)>0:
        last_point = explore_list.pop(0) # リストから探索する位置を取得

        # 上下左右のマスの座標を取得
        # update explore list
        around_list = last_point.add_around()
        
        # 探索待ちリスト（explore_list）に追加
        ## ただし，障害物や外周，すでに通ったマスなどは探索領域から除外する
        for i in range(len(around_list)):
            p = around_list[i]

            # if the point is outside of maze or on object, skip
            if maze[p.x][p.y] == 9:
                continue
            # if the point is already on the explore list, skip
            elif maze[p.x][p.y] == 2:
                continue

            # if the point is goal, FINISH
            elif maze[p.x][p.y] == 1:                
                trace_list.append(p)
                route_list = RouteRecord(trace_list)
                return route_list, trace_list

            # if the point should be explored, add to the explore list
            elif maze[p.x][p.y] == 0:
                maze[p.x][p.y] = 2
                # update explore list
                explore_list.append(p)
                trace_list.append(p)
                continue

            # if the point is arounded by objects&explored point, skip 
            else:
                # print("No route to go")
                continue
    return 0

```

## 経路表示
探索の結果，*Search( )*関数からは ゴールからスタートまでの最短経路を格納している*route_list*と，探索したすべての座標を記録した*trace_list* が得られる．

ここでは，FuncAnimationを使って以下のような表示を行うgifを作成する．
1. 探索された座標としてtrace_listに記録された順にグラフに表示
2. trace_listの全要素を表示したら，最後に最短経路を表示する

```python:main.py
class Maze():
    ・・・
    def set_maze_graph(self):
        fig, ax = plt.subplots(figsize=self.map_size)
    
        ax.set_xticks(list(range(self.map_size[0]+1)))
        ax.set_yticks(list(range(self.map_size[1]+1)))
        ax.grid()
        ax.set_aspect('equal')

        # set maze
        for x in range(self.map_size[0]):
                for y in range(self.map_size[1]):
                    if self.maze[x][y] == 9:
                        r = patches.Rectangle( xy=(x,y), width=1, height=1, color="grey", alpha=0.5) # 四角形のオブジェクト
                        # 4. Axesに図形オブジェクト追加・表示
                        ax.add_patch(r)
        # set star&goal position
        s2g = [[self.start.convert_coordinate().x, self.goal.convert_coordinate().x],
                 [self.start.convert_coordinate().y, self.goal.convert_coordinate().y]]
        
        ax.scatter(s2g[0], s2g[1], c="orange", marker="*", s=350)
        return fig, ax

    def animation(self, frame):
        ax.clear()
        # draw maze
        for x in range(self.map_size[0]):
                for y in range(self.map_size[1]):
                    if self.maze[x][y] == 9:
                        r = patches.Rectangle( xy=(x,y), width=1, height=1, color="grey", alpha=0.5) # 四角形のオブジェクト
                        # 4. Axesに図形オブジェクト追加・表示
                        ax.add_patch(r)
        # set start&goal
        s2g = [[self.start.convert_coordinate().x, self.goal.convert_coordinate().x],
                 [self.start.convert_coordinate().y, self.goal.convert_coordinate().y]]
        
        ax.scatter(s2g[0], s2g[1], c="orange", marker="*", s=350)
        plt.text(self.start.x + 0.7, self.start.y + 0.7, "S")
        plt.text(self.goal.x + 0.7, self.goal.y + 0.7, "G")
        
        # 探索したすべてのマスを順に表示
        # trace the route
        for i in range(frame+1):
            ax.scatter(trace_list[i].convert_coordinate().x, trace_list[i].convert_coordinate().y, c="blue", marker="x", zorder=0)
        
        # 最短経路の表示
        # show route list at the end
        if frame+1 == len(trace_list):
            for i in range(1,len(route_list)-1):
                ax.scatter(route_list[i].convert_coordinate().x, route_list[i].convert_coordinate().y, c="red", marker="x", zorder=1)
        
        ax.set_xlim([0, self.map_size[0]])
        ax.set_ylim([0, self.map_size[1]])
        ax.set_aspect('equal')

# <<<--- ここからがメイン関数部分！ --->>>#
if __name__ == '__main__':
    create_maze = Maze()
    maze, start = create_maze.generation_map() # generate map
    fig, ax = create_maze.set_maze_graph() # グラフの初期設定
    route_list, trace_list= Search(start)  # 探索
    # アニメーション化
    anim = FuncAnimation(fig, create_maze.animation, frames=len(trace_list), interval=100, repeat=False)
    anim.save("test.gif", writer="pillow")
    plt.show()
```