JH721 SW자율차 [Path Planning] //14주차-1

pacman을 예로 드름
https://www.google.com/logos/2010/pacman10-i.html

Nodes : world configurations
Arcs : represent successors
Goal test : set of goal nodes

import numpy as np

import time
import matplotlib.pyplot as plt
from IPython.display import clear_output
from math import sqrt


class Node():
    def __init__(self, parent=None, position=None):
        self.parent = parent
        self.position = position

        self.g = 0
        self.h = 0
        self.f = 0

    def __eq__(self, other):
        return self.position == other.position


def astar(maze, start, end):
    start_node = Node(None, start)
    start_node.g = start_node.h = start_node.f = 0
    end_node = Node(None, end)
    end_node.g = end_node.h = end_node.f = 0

    # A*: (지금까지 온 코스트 + 앞으로 볼 코스트에 대한 예측)을 "최소화"하는 방향으로 search를 할 예정
    # 최소화 : 도달할 수 있는 노드에 대한 cost들을 전부 탐색해본 이후, 그들 중에서 최고인 것을 선택
    open_list = []  # 도달할 ㅅ ㅜ있는 노드에 대한 cost들을 전부 탐색 하는 후보군
    closed_list = []  # 지금까지 지나온 노드를 저장하는 리스트

    # Add the start node
    open_list.append(start_node)

    # Loop until you find the end
    # A* 알고리즘이 끝나는 두가지 경우
    # 1. 도달가능한 모든 후보군을 탐색한 경우
    # 2. 최적의 cost를 가진 경로를 찾는 경우

    while len(open_list) > 0:

        # 후보군 중에서, 가장 cost가 낮은 노드를 찾음
        current_node = open_list[0]
        current_index = 0
        for index, item in enumerate(open_list):
            if item.f < current_node.f:
                current_node = item
                current_index = current_index
        # current_node, current_index : open_list중에서 cost가 가장 작은 node로 설정
        # 후보군 중에서, 지금 탐색할 노드를 선택해습니다.

        open_list.pop(current_index)  # 걔는 이제 탐색 대상이지, 더이상 후보군이 아니기 때문에, open_list에서 해단 node를지움
        closed_list.append(current_node)  # 탐색ㄱ하고자하는 노드는, 다음 탐색하지 않을 것이기 때문에, close list에 추가

        # Found the goal
        if current_node == end_node:  # goal state를 찾는 경우
            path = []  # goal state까지의 추적하기 위함
            current = current_node
            while current is not None:
                path.append(current.position)
                current = current.parent
            return path[::-1]

        # 지금까지 본 것들중, 아직 확인하지 않은 것:
        # 1. open_list는 어떤 식으로 축가가 되는가
        # 2. clised_list 는 어떤 식으로 탐색에서 예왹가 되는가
        # 3. parent는 어떻ㄱ게 설정하는가
        # 4. 어떤 방식으로 탐색을 할것인가(f,g,h를 어떻게 지정해줄까)

        # Generate children
        # 탐색할 노드를 선책한 이후엔, 어떤것을 하나? -> 주변 노드를 살펴본다.
        children = []
        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) - 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)

        for child in children:
            for closed_child in closed_list:
                if child == closed_child:
                    continue

            child.g = current_node.g
            child.h = sqrt((child.position[0]) ** 2) * ((child.position[1] - end_node.position[1]) ** 2)
            child.f = child.g + child.h

            for open_node in open_list:
                if child == open_node and child.g > open_node.g:
                    continue

            open_list.append(child)


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

start = (0, 0)
end= (9, 9)

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