はじめに

こんにちは、42tokyo Advent Calendar 2021の10日目を担当する、在校生のものです。
今回は経路探索について勉強する機会があったので、その実装について、経路探索これからはじめたいなぁという方向けに書いていきたいと思います。
この記事の内容は以下になります。

題材はグリッド状の重みなしの経路探索
グリッド状のマップの自動生成プログラム
探索した経路を移動している様子をgifで表示
簡単なアルゴリズム（BFS的なやつ）で実践

この記事を読むと、最終的には以下のようなコードで、経路探索を可視化して自分のアルゴリズムを確認できるようになります。

from lib.algolism import pathfinding_algolism1
from lib.map_generation import generation_map
from lib.map_gif_draw import arrays_to_gif, make_track_arrays_all
import numpy as np

map_size = (6, 6)
object_num = 10
map_array = generation_map(map_size=map_size, object_num=object_num)
count_array, goal_list = pathfinding_algolism1(map_array)
if len(goal_list) > 0:
    print(len(goal_list))
    array_list = make_track_arrays_all(goal_list, map_array)
    arrays_to_gif(array_list, save_path='sample.gif', max_v=4, show=True, duration=300, img_size=(128, 128))

これを実行すると

こんな感じでgifで求めた最短経路を表示してくれます。
今回はこのコード上でimportしているそれぞれの関数の解説という感じになっています。

ちなみに上のコードは以下のgithub上にすべて載せているので、もしよろしければご覧ください。
https://github.com/kotabrog/Introduction_to_pathfinding

環境

あまり環境依存なものはないと思いますが、念のため私の環境を書いておきます。

Windows10
Python: 3.8
numpy: 1.19.5
Pillow: 8.3.1

マップの自動生成

まずは実験をするうえでマップが自動で作れると楽なので、自動生成プログラムを作っていきます。
コードとしては以下のようになっています。

import random
import itertools
import numpy as np


def choice_and_del(choice_list, choice_num):
    ret_list = []
    for _ in range(choice_num):
        index = random.choice(range(len(choice_list)))
        ret_list.append(choice_list.pop(index))
    return ret_list


def generation_map(map_size=(6, 6), object_num=0):
    map_array = np.zeros(map_size)
    map_list = list(itertools.product(range(map_size[0]), range(map_size[1])))

    start_coords = choice_and_del(map_list, 1)
    map_array[start_coords[0][0], start_coords[0][1]] = 1

    goal_coords = choice_and_del(map_list, 1)
    map_array[goal_coords[0][0], goal_coords[0][1]] = 3

    if object_num > 0:
        object_coords = choice_and_del(map_list, object_num)
        for object_coord in object_coords:
            map_array[object_coord[0], object_coord[1]] = 2

    return map_array

generation_map関数は、map_sizeとobject_numを受け取ります。
mapは以下のように、スタートが1、ゴールが3、障害物が2のnumpy.ndarray形式の2次元配列として出てきます。

[[2. 0. 2. 2. 0. 2.]
 [0. 0. 0. 0. 2. 0.]
 [0. 0. 0. 0. 0. 0.]
 [0. 0. 0. 2. 0. 2.]
 [0. 0. 2. 0. 0. 0.]
 [2. 0. 1. 2. 0. 3.]]

ちなみにこの図はさきほどのgifのマップになります。

関数の中身は単純で、generation_mapの最初の部分

    map_array = np.zeros(map_size)
    map_list = list(itertools.product(range(map_size[0]), range(map_size[1])))

で関数の返り値として返すmap_arrayと、mapの座標が入ったリストmap_listを作成します。
この後は順次スタート、ゴール、オブジェクトの位置をランダムに決めていくのですが、もう埋めた場所はこのリストから削除していくことで、同じ場所を再び指定することがないようにしています。

スタート、ゴール、オブジェクトの位置は、以下のような形でchoice_and_del関数を使って指定していっています。

    start_coords = choice_and_del(map_list, 1)
    map_array[start_coords[0][0], start_coords[0][1]] = 1

choice_and_del関数の中では、choice_listからchoice_num個取り出す処理をしています。
choice_list.pop(index)で取り出しつつ削除も行っています。

def choice_and_del(choice_list, choice_num):
    ret_list = []
    for _ in range(choice_num):
        index = random.choice(range(len(choice_list)))
        ret_list.append(choice_list.pop(index))
    return ret_list

作成したマップを複数並べてgifにして可視化

続いてgifにする部分です。
さきほどの2次元配列のリストを作って、それをgifにしていきます。
まずはコードを載せておきます。

import numpy as np
from PIL import Image
import base64
from IPython import display as dd


def array_normalize(array: np.ndarray, max_v):
    array = array / max_v
    array *= 255
    return array.astype(np.uint8)


def array_to_img(array: np.ndarray, max_v=2, img_size=(64, 64)):
    img = Image.fromarray(array_normalize(array, max_v)).resize(img_size, Image.NEAREST).convert('P')
    return img


def arrays_to_gif(array_list,
                  save_path='sample.gif',
                  max_v=4,
                  img_size=(64, 64),
                  show=False,
                  duration=1000):
    images = [array_to_img(array, max_v, img_size) for array in array_list]
    images[0].save(save_path,
               save_all=True, append_images=images[1:], optimize=False, duration=duration, loop=0)
    if show:
        with open(save_path, "rb") as f:
            b64 = base64.b64encode(f.read()).decode("ascii")

        display(dd.HTML(f'<img src="data:image/gif;base64,{b64}" />'))

arrays_to_gif関数のarray_listとして、さきほど作成した2次元配列のリストをわたしてあげれば、一つ一つの2次元配列のマップを画像に変換して、そのあとでgifにしてくれます。
arrays_to_gif関数のそれぞれの引数の役割は以下のようになっています。

array_list: さきほど作成した2次元配列のリストをわたす
save_path: gif画像として保存する先のパス
max_v: map内の値の最大値。この値でマップを正規化（0~1の値にすること）する
img_size: gifにする際のサイズ
show: Trueにするとnotebook形式のときにその場で表示する
duration: gifの各フレームの表示時間。単位はミリ秒

arrays_to_gif関数の中ではまず最初にarray_listに入っているマップをarray_to_img関数で画像のデータに変換しています。

images = [array_to_img(array, max_v, img_size) for array in array_list]

さらにarray_to_img関数では、まず最初にarray_normalize関数で2次元配列のマップを0~255の値に変換しています。

def array_normalize(array: np.ndarray, max_v):
    array = array / max_v
    array *= 255
    return array.astype(np.uint8)

def array_to_img(array: np.ndarray, max_v=2, img_size=(64, 64)):
    img = Image.fromarray(array_normalize(array, max_v)).resize(img_size, Image.NEAREST).convert('P')
    return img

0~255の値に変換したらImage.fromarrayでPillowの画像形式に変換して、そのあとresizeで画像サイズを変更。
そしてそのあとconvert('P')でパレットモードにしています。
（パレットモードにしないと、どこかで不具合が起きたのですが、このコードがちょっと前に書いたやつで、どこに不具合が起きたか忘れてしまいました……）
resize時の注意として、Image.NEARESTにすると、近くの色と同じ色にしてリサイズしてくれるので、格子状のマップっぽくなりやすくなります。（ほかのリサイズだと色がグラデーションになってしまうことがあります）

array_to_img関数で画像に変換したら、次はgif画像に変換します。

images[0].save(save_path,
               save_all=True, append_images=images[1:], optimize=False, duration=duration, loop=0)

gif画像への変換については、こちらがわかりやすかったです。

そして、notebook形式の際に表示する場合についてですが、以下のように変換しています。

    if show:
        with open(save_path, "rb") as f:
            b64 = base64.b64encode(f.read()).decode("ascii")

        display(dd.HTML(f'<img src="data:image/gif;base64,{b64}" />'))

こちらも詳しくは以下のURLをご覧ください。

アルゴリズムの準備として

それでは今まで作成したマップの自動生成とgif画像への変換がうまくいっているかの確認として、簡単なアルゴリズムで経路探索を行いたいのですが、その前にPositionクラスを作成します。
このクラスは、その地点のx, y座標を持ったクラスで、コードをわかりやすくする目的で使っています。

class Position:
    def __init__(self, x: int, y: int):
        self.x = x
        self.y = y

    def __repr__(self):
        return "<position: [{}, {}]>".format(self.x, self.y)

    def clone(self):
        return Position(self.x, self.y)

    def __hash__(self):
        return hash((self.x, self.y))

    def __eq__(self, other):
        return self.x == other.x and self.y == other.y

    def get_adjacent_list(self):
        adjacent_pos_list = [
            Position(self.x, self.y - 1),
            Position(self.x + 1, self.y),
            Position(self.x, self.y + 1),
            Position(self.x - 1, self.y)
        ]
        return adjacent_pos_list

    def is_adjacent(self, other) -> bool:
        adjacent_pos_list = self.get_adjacent_list()
        return other in adjacent_pos_list

今回は使用していませんが、__hash__を定義することでpythonの辞書型のkeyとして使用できたり、__eq__を作成することで2つのPositionが等しいかどうかを見ることができるようにしています。

また、get_adjacent_listはそのPositionの前後左右を返すリストになっています。

BFS（幅優先探索）

それではいよいよ基本的な経路探索アルゴリズムの実装です。
今回は幅優先探索（breadth first search）的なものを実装しました。

幅優先探索の詳しい説明についてはこちらをご覧ください。

ちなみにこちらではキューというデータ構造を使っていますが、今回はキューは使っていません。

それではさっそくコードを見ていきます。

import copy
import numpy as np

from .position import Position

def in_map(map_size, position: Position) -> bool:
    height, width = map_size
    return 0 <= position.x and position.x < width and\
            0 <= position.y and position.y < height


def pathfinding_algolism1(map_array: np.ndarray):
    map_size = map_array.shape
    count_array = np.full_like(map_array, -1)

    explore_list = []
    goal_point = np.where(map_array == 3)
    goal_point = Position(goal_point[1][0], goal_point[0][0])
    start_point = np.where(map_array == 1)
    start_point = Position(start_point[1][0], start_point[0][0])
    explore_list.append([start_point])
    count_array[start_point.y][start_point.x] = 0

    turn = 0
    done = False
    goal_list = []
    while True:
        turn += 1
        temp_explore_list = []
        for explore in explore_list:
            last_point = explore[-1]
            adjacent_list = last_point.get_adjacent_list()
            for p in adjacent_list:
                if p == goal_point:
                    done = True
                elif not in_map(map_size, p) or map_array[p.y][p.x] == 2:
                    continue
                if count_array[p.y][p.x] != -1 and count_array[p.y][p.x] < turn:
                    continue
                count_array[p.y][p.x] = turn
                append_list = copy.copy(explore)
                append_list.append(p)
                temp_explore_list.append(append_list)
                if p == goal_point:
                    goal_list.append(append_list)
        explore_list = temp_explore_list
        if done or len(explore_list) == 0 or turn > 100:
            break
    return count_array, goal_list

pathfinding_algolism1に最初に作ったマップの２次元配列を渡すと、count_array, goal_listが返ってくるようになっています。
count_arrayは以下のように、どこに何ステップでいけるかをゴールにたどり着く過程で保存していったものになります。

[[-1.  6. -1. -1. -1. -1.]
 [ 6.  5.  6.  7. -1.  9.]
 [ 5.  4.  5.  6.  7.  8.]
 [ 4.  3.  4. -1.  8. -1.]
 [ 3.  2. -1. 10.  9. 10.]
 [-1.  1.  0. -1. 10. 11.]]

-1になっているところは、障害物があるか、まだ行っていない場所です。
また、0になっているところがスタート地点になります。
このcount_arrayは最初に出てきたgif画像でのものですが、count_array内の最大値が11であることから、ゴールまで11ステップかかったことが読み取れるようになっています。

また、goal_listは以下のようになっています。

[[位置A1, 位置A2, ..., 位置An],
[位置B1, 位置B2, ..., 位置Bn],
...]

このようにリストの中に位置のリストがある構造になっています。
この位置のリストは、スタートからゴールまでに通ってきた位置のリストになっています。

スタート=位置A1 -> 位置A2 -> ... -> 位置An=ゴール]

今回のコードでは、最短でゴールまでたどり着けるすべての経路を求めるようになっているので、goal_listは位置のリストのリストになっています。

さて、それではpathfinding_algolism1関数の中身を見ていきましょう。
まず最初の部分です。

    map_size = map_array.shape
    count_array = np.full_like(map_array, -1)

    explore_list = []
    goal_point = np.where(map_array == 3)
    goal_point = Position(goal_point[1][0], goal_point[0][0])
    start_point = np.where(map_array == 1)
    start_point = Position(start_point[1][0], start_point[0][0])
    explore_list.append([start_point])
    count_array[start_point.y][start_point.x] = 0

ここでは最後まで使っていくリストなどの定義をしています。
start_point, goal_pointはその名の通りスタートとゴールの位置を表しますが、先ほど紹介したPositionクラスを使用しています。
explore_listは今探索している経路のリストで、goal_listと同じように以下のような構造になっています。

[[位置A1, 位置A2, ..., 位置An],
[位置B1, 位置B2, ..., 位置Bn],
...]

この時点ではexplore_list=[[[start_point]]]になっています。

次にwhile文の中に入っていきますが、ここは1ステップで1周回るようになっています。
while文の中で最初に出てくるfor文は、explore_list……つまり今探索している経路ごとに回っていくことになります。

        for explore in explore_list:
            last_point = explore[-1]
            adjacent_list = last_point.get_adjacent_list()

last_pointはexploreの一番後ろの要素で、これは直前のステップでの位置になります。
この直前の位置から、get_adjacent_list()関数で前後左右の位置のリストを取得します。

            for p in adjacent_list:
                if p == goal_point:
                    done = True
                elif not in_map(map_size, p) or map_array[p.y][p.x] == 2:
                    continue
                if count_array[p.y][p.x] != -1 and count_array[p.y][p.x] < turn:
                    continue

次のfor文は、前後左右の位置ごとに回していきます。
まずその位置がゴールだった場合はdone = Trueとして、while文を後で抜けられるようにフラグをセットします。
その次に、そこがいける場所かどうかを、２つの観点で見ています。

その位置がマップ内かどうか：in_map(map_size, p)
その位置が障害物であるか：map_array[p.y][p.x] == 2

もしちゃんといける場所であれば、次のif文に進み、そこが行ったことがある場所か、もし行ったことがあるのであれば、今より短いステップで到着できているかどうかを見ます。

                count_array[p.y][p.x] = turn
                append_list = copy.copy(explore)
                append_list.append(p)
                temp_explore_list.append(append_list)
                if p == goal_point:
                    goal_list.append(append_list)

もし行ったことがなかったり、今と同じステップ以上のステップ数でそこにたどり着いているのであれば、count_arrayの値を更新し、今見ているexploreリストの後ろにこの位置を追加して次のステップで使う探索経路のリストに追加します。
また、もしゴールにたどり着いているようであれば、関数の返り値であるgoal_listにこの探索経路を追加します。

        explore_list = temp_explore_list
        if done or len(explore_list) == 0 or turn > 100:
            break

そしてこの2つのfor文を終えたら、temp_explore_listを新たなexplore_listとして、次のステップに移行します。
もしここでゴールにたどり着いていたり、explore_listが空でもう探索する場所がなかったり、ステップがかかりすぎていた場合はwhile文を抜けるようにします。

少し説明が長くなってしまいましたが、これでアルゴリズム部分も終了となります。

位置のリストをマップに変換

最後に、上のアルゴリズムで得た位置のリストをマップのリストに変換する部分を作成していきます。

def make_track_arrays(point_list, map_array, v=4):
    map_arrays = []
    for p in point_list:
        array = map_array.copy()
        array[p.y][p.x] = v
        map_arrays.append(array)
    return map_arrays


def make_track_arrays_all(goal_list, map_array, max_track=30, v=4):
    map_arrays = []
    for i, point_list in enumerate(goal_list):
        if i >= max_track:
            break
        map_arrays += make_track_arrays(point_list, map_array, v)
    return map_arrays

make_track_arraysは一つの位置のリストを、arrays_to_gifで変換できる形に変えています。
また、make_track_arrays_allは、位置のリストのリスト（pathfinding_algolism1の返り値のgoal_list）を、同様にarrays_to_gifで変換できる形に変えています。

ということでこれですべての関数の説明が終わりました！
これらの関数を読み込むことで、最初にあげた以下のコードを実行することができます。（フォルダの構成はgithubをご覧ください）

from lib.algolism import pathfinding_algolism1
from lib.map_generation import generation_map
from lib.map_gif_draw import arrays_to_gif, make_track_arrays_all
import numpy as np

map_size = (6, 6)
object_num = 10
map_array = generation_map(map_size=map_size, object_num=object_num)
count_array, goal_list = pathfinding_algolism1(map_array)
if len(goal_list) > 0:
    print(len(goal_list))
    array_list = make_track_arrays_all(goal_list, map_array)
    arrays_to_gif(array_list, save_path='sample.gif', max_v=4, show=True, duration=300, img_size=(128, 128))

さいごに

今回経路探索の入門としてアルゴリズム寄りというよりは可視化寄りの記事を書いたのは、２つの理由があります。
まず１つ目は、やっぱり目で見えた方がテンションが上がるし、自分のやったことを人に見せる時にもインパクトがあるからです。
それに自分で作ったものが合っているかの確認にもなりますし、プログラムを書くときにはどのように可視化して結果を確認するか……というのを最初に考えることは多いかもしれないなと思ったので、入門としての記事を可視化寄りにしました。

もう一つの理由は、アルゴリズムの入門記事はとてもたくさん良いものがあったからです。
今からそれらに勝とうとは微塵も思えませんでした笑。
この記事を読んで、経路探索の勉強をしてみたい！と思った方は、ぜひ以下の記事も読んでみてください。

また、今回のように記事にすることもある意味可視化の一つだと思うので、Advent Calendarのような企画はとてもいいものだなぁと思ったりします。
次の担当はuhyotさんが「コンテナについて」の記事を書いてくださっているので、ぜひそちらもご覧ください。