14주차-PathPlanning(1)

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

class Node():
# A node class for A* Pathfinding

  def __init__(self, parent=None, position = None):
    # Node라는 객체를 정의함과 동시에 실행되는 함수가 __init__
    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, star, end):
  # Returns a list of tuples as a path from the given start to the given end in the given maze

  # Create start and end node
  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.h = 0

  # Initialize both open and closed list
  # A* : (지금까지 온 코스트+ 앞으로 볼 코스트에 대한 예측)을 최소화하는 방향으로 search 예정
  # 최소화 : 도달할 수 있는 노드에 대한 cost들을 전부 탐색해본 이후, 그둘 중에서 최소인것을 선택
  open_list = [] # 도달할 수 있는 노드에 대한 cost들을 전부 탐색하는 후보군
  closed_list = [] # 지금까지 지나온 노드를 저장하는 리스트(다시 방문하지 않을 것, A*는 기본적으로 cost가 가장 낮은 애들부터 탐색)

  # Add the start node
  open_list.append(start_node)

  # Loop until you find the end
  # A* 알고리즘이 끝나는 두 가지 경우
  # 1. 도달가능한 모든 후보군을 탐색한 경우
  # 2. 최적의 cost를 가진 경로를 찾은 경우
  while len(open_list) > 0: # 1번 케이스에 대한 예외처리

    # 후보군(open_list)중에서 가장 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 = index
    # current_node, current_index : open_list 중에서 cost(f값)이 가장 작은 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: # start node의 parent는 None으로 지정해놨었음
        path.append(current.position) # 현재 position을 넣어요
        current = current.parent # 해당 node에 도달하기 이전에, 어떤 node로부터 왔는지를 찾습니다. -> 그 노드의 position은
        # -> 이 과정이 결국, goal state에서부터 start node까지 도달하는 경로를 역추적하는 과정
      return path[::-1] # Return reversed path

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

    # Generate children
    # 탐색할 노드를 선택한 이후엔, 어떤것을 하냐? -> 주변 노드를 살펴본다.
    children = []
    for new_position in [(0, -1),(0, 1), (-1, 0), (1, 0)]: # Adjacent squares
    
      # Get node position
      node_position = (current_node.position[0] + new_position[0], current_node.position[1] + new_position[1])
      
      # Make sure within range
      if (node_position[0] > (len(maze) - 1) or node_position[0] < 0) or (node_position[1] > (len(maze[0]) -1) or node_position[1] < 0):
        continue

      # Make sure walkable terrain
      if maze[node_position[0]][node_position[1]] != 0:
        continue

      # Create new node
      # 이 단계를 거치기 전까지, 후보군은 그냥 좌표값
      # 해당 후보군에 대한 node를 정의해줌으로써, 그 node에 대한 cost/position/parent에 대한 정보를
      # 묶어서 확인할 수 있게 됌
      new_node = Node(current_node, node_position)

      # Append
      children.append(new_node)

    # -> children : 탐색하는 현재 node(current node)의 8방 이웃 노드들 중에서, 벽이 아니고, 메이즈 바깥으로 넘어간 범위도 아닌,
    # 단순하게 생각했을 때 도달가능한 node를 선택하는 단계

    # children 중에서, open list에 어떤걸 추가할거냐 라는건 뒤에서 해결합니다. (예컨대, 방문했던 node(closed list)에 대한 예외처리)
    for child in children:

      # Child is on the closed list
      for closed_child in closed_list:
        if child == closed_child:
          continue # child가 closed list 안에 포함되어 있다면 넘어간다
      
      # closed list 안에 포함되어 있지않다면, 해당 child는 open list에 추가되어서, 다음 단계의 탐색에서 활용될 것
      # openlist에 추가되기 위해서 (다음 단계에서 탐색후보군으로써 활용되기 위해서), cost를 미리 계산해둘겁니다.
      # Create the f, g and h values
      child.g = current_node.g + 1 # 실제로 지나오면서 소요됬던 cost
      child.h = sqrt((child.position[0] - end_node.position[0]) ** 2) +  ((child.position[1] - end_node.position[1]) ** 2) # goal 까지 도달하기 위해 소요될 것으로 예상되는 cost(heuristic)
      child.f = child.g + child.h # 이 두개를 합친 값을 cost로 활용함으로써, A* 알고리즘을 실행시킬겁니다

      # Child is already in the open list
      for open_node in open_list:
        if child == open_node and child.g > open_node.g: # 현재 탐색하는 노드에 대한 cost, open_node.g : 기존 open list에 포함되어 있는 노드에서의 cost
          continue

      # Add the child to the open list
      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)