経路計画を勉強する第一歩として，幅優先探索について学ぶ．
流れとしては以下の通り，
- 迷路の生成：障害物とスタート＆ゴール地点をランダムに設定し，探索する迷路の生成する
- 探索：障害物を避けながら，スタート→ゴールまでの経路を探索する
- 経路表示：探索結果を探索領域および最短経路として表示する
経路表示をターミナル上に文字で行っているものが多かったのですが，やっぱりちゃんとグラフで＆アニメーションで表示された方が気分も上がりますよね！
コードの全体像
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」
迷路生成の詳細については，☟を参照．
探索
こちらのサイトをもとに作成．
深さ探索では，スタート地点→スタート地点の（左右上下）周囲4点→各4点の周囲4点．．． というように，スタート地点からの移動回数（深さ）が同じ点を中心に探索領域を広げていく．
このとき，一番最初にゴールにたどり着いた経路が必ず最短経路となる．
以下のコードは，
- スタートからゴールまで探索：Search( )
ただし，障害物や外周，すでに通ったマスなどは探索領域から除外する - ゴールから順に一つ前で通ったマスを遡ることで，最短経路を取得：RouteRecord( )
以上，2つの構成からなる．
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を作成する．
- 探索された座標としてtrace_listに記録された順にグラフに表示
- trace_listの全要素を表示したら，最後に最短経路を表示する
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()