ROS2로 turtlebot3 제어하기 4장. map파일을 가지고 전역경로 생성하기

이번 글은 별도의 참고자료가 존재하지 않고, 참고하는 블로그가 있는 경우 바로 다음에 표시를 하고 지나가도록 하겠습니다.

---

이전 시간에 파이썬 가상환경을 만들었습니다.

그리고 맵의 색에 따라서 0과 1로 구분짓자는 것까지 이해했습니다. 이번시간에는 아주 간단하게 전역경로를 생성하고 이를 따라 움직이는 turtlebot3를 만들어보고자 합니다.

저는 map파일을 참고해서 경로를 계획하게 할 것입니다. 정말 간단하게는 픽셀단위로 잘라서 경로를 계획할 수 있습니다. 그러나 픽셀단위로 자르면 그림상에서 이해하기가 조금 어려워지기 때문에 사각형 단위로 자르도록 하겠습니다. 우선은 알고리즘이 동작하게 만드는 것이 우선입니다. 픽셀단위로 자르는 방법은 이후에 진행하도록 합시다.

gazebo를 실행하면 바닥에 가상의 사각형이 생기는 것을 알 수 있습니다.  

이 가상의 선으로 칸을 잘라낼 것입니다.  우선 로봇의 위치를 조금 바꾸도록 합시다.

칸의 중앙에 들어갔으면 합니다. 그러니 0.5씩 값을 주도록 합시다. 그리고 로봇이 위를 보고 있는 것도 조금 문제가 있다고 생각합니다. 이거에 대한 부분도 조금 알아보도록 하겠습니다.

가제보 패키지에 들어가서 turtlebot3_duworld.launch.py 해당 파일의 값을 다음처럼 변경합니다. 

그리고 바라보는 방향은  spwan_turtlebot3.launch.py 파일을 찾아야합니다. 이 파일은 같은 폴더 내에 위치합니다.

해당 파일의 57번째 줄을 새로 개행하고

			,
'-Y', '1.57'

다음을 추가합니다. 쉼표를 빼먹지 않도록 합니다.

다음처럼 설정이 완료되었기 때문에 이제 turtlebot3의 코드는 모두 끝났습니다.

 

그리드 분할이란?

위에서 언급한 것처럼 이미지를 잘라서 그 이미지의 색에 따라서 맵을 만든다는 개념을 적용하기 위해서는 우선 이미지를 분리할 수 있어야합니다. 간단하게 이미지를 사각형으로 분할하는 것을 그리드 분할이라고 합니다. 그림을 그릴 수 있으면 좋을텐데

다음의 표로 설명을 합시다.

다음의 사각형이 있다고 합시다. 2개의 행, 3개의 열로 분할을 하면 

다음처럼 분할할 수 있습니다. 그러면 6개의 조각들이 만들어집니다. 이게 그리드 분할입니다. 

(사실 픽셀단위로 분할해서 사용할 예정이라면 여기까지는 필요없습니다.)

그러면 이제 opencv를 활용해서 이미지를 분할시켜 봅시다. 다시 VScode를 켜봅시다. map파일과 같은 경로에 있어야하기 때문에 그냥 편하게 home에서 작업하는 것을 추천합니다.

그래서 여기까지 진행하고 끝이났었습니다.  우선 모듈들을 불러와 줍시다.

import cv2
import yaml
import sys
import numpy as np
import matplotlib.pyplot as plt

다음의 모듈들을 사용합니다.  cv2가 opencv가 될 것이고, matplotlib가 이미지를 보여주기 위해 불러온다고 보시면 됩니다.

# 지도 이미지 로드
pgm_img = cv2.imread('./map.pgm', cv2.IMREAD_GRAYSCALE)
# PNG로 저장
cv2.imwrite('./map.png', pgm_img)

우선 지도 이미지를 불러와서 png로 저장을 해야합니다. 그래야 이미지처리가 가능합니다.

이 과정이 끝나면 홈에 png로 된 맵파일이 존재하는 것을 확인하실 수 있습니다. 이제 이 파일을 불러와서 한 번 확인을 해보도록 합시다.

map_img = cv2.imread('./map.png',cv2.COLOR_BGR2GRAY )
plt.imshow(map_img)

다음처럼 읽어오는 것을 확인할 수 있습니다. 뭐라더라.. 노란색이면 도달할 수 있는 범위를 뜻하고, 초록색이면 도달하지 못하는 공간을 뜻합니다.

여튼 이제 이를 분할시켜야합니다. 가제보를 켜보고 내가 몇칸을 하려했는지 확인해보고 오겠습니다.. 12by12 짜리 입니다. 그러니 그리드분할을 이용해서 12, 12로 나눠버립시다.

우선 그리드 알고리즘을 함수로 나타내면 다음과 같습니다.

def image_Grid(h, w, img):
    img_height, img_width = img.shape
    grid_size = (h, w)  # 세로, 가로
    
    # 그리드 영역 크기 계산
    cell_height = img_height // grid_size[0]
    cell_width = img_width // grid_size[1]
    
    map_cost = []
    map_col = []
    
    #각각의 그리드 영역에서 흰색 픽셀 비율 계산
    for i in range(grid_size[0]):
        map_col = []
        for j in range(grid_size[1]):
            # 그리드 영역의 범위를 지정하는거고
            cell_top = i * cell_height
            cell_left = j * cell_width
            cell_bottom = cell_top + cell_height
            cell_right = cell_left + cell_width
         
            if i == h-1:
                cell_bottom = img_height
                           
            # 그 맵의 모든 픽셀을 가져와서
            cell = img[cell_top:cell_bottom, cell_left:cell_right]
            wall = 0
            road = 0
            for row in range(len(cell)):
                for col in range(len(cell[0])):
                
                    if cell[row][col] <= 210:
                        wall += 1;
                    else:
                        road += 1
            if road > wall:
                map_col.append(0)
            else:
                map_col.append(1)
        map_cost.append(map_col)   
    return map_cost, cell_height, cell_width

간단하게 우리가 구해야할 map은 map_cost라는 변수에 저장됩니다. 그리고 그리고에서 한 행씩 처리하기 때문에 map_col 리스트를 계속해서 추가해나가는 방식으로 구현됩니다.

그래서 for문을 돌면서 그리드로 분할을 진행하고 그리드 영역의 범위의 픽셀값들을 확인해보았을 때, 픽셀이 흰색이 아니면 wall에 추가하고 흰색이면 road에 추가합니다. 그렇게 했을 때 road가 wall보다 많으면 그 그리드는 비어있는 도로임으로 움직일 수 있다고 판단을 합니다.

my_map, cell_height, cell_width = image_Grid(12, 12, map_img)
for i in my_map:
    print(i)

그래서 다음을 입력해보고 my_map을 확인해봅니다. 잘 나오는지.. 결과를 확인해보니 잘 나오지 않네요.. 맵파일을 더 손봐야할 것 같습니다.

맵을 벗어난 범위를 이미지 편집툴을 사용해 자르겠습니다.

그리드에서 자르게 된다면 만약 마지막 남은 칸은 값이 이상하게 들어가게 됩니다. 25를 3으로 나누면 마지막 그리드는 1이 되는 느낌이라 생각하면 됩니다. 

그리드로 맵을 나누는 방법은 좋은 방법이 아니라는 것을 깨닫게 된거 같네요. 이미지 편집툴을 사용해서 12의배수가 되도록 이미지의 픽셀값을 조절해봅시다. (이미지를 잘라봅시다.)

240 by 240 크기로 맵을 조절했더니 알맞게 분할되었습니다..

후.. 이거 때문에 1시간이 날아갔네요... 다음 시간에는 그냥 픽셀단위에서 바로 게산을 때리는 거로 하겠습니다.

행렬과 맵을 한 번 비교해주시기 바랍니다. 벽으로 막혀있는 곳은 정확하게 1로 표시되어 있고, 막히지 막히지 않은 부분은 0으로 표시되었습니다.

결국 그리드로 나눌 때 딱 나누어 떨어지게 픽셀값을 조절할 수 있다면 정확한 맵행렬을 얻을 수 있다 이런 뜻이군요.. 머리아프네요

 

이제 전역경로를 생성해보자.

경로를 생성하는 알고리즘은 우선 A* 알고리즘을 사용하도록 하겠습니다. 이 알고리즘에 대한 해설은 로봇을 움직인 이후에 자세히 다루도록 하겠습니다.

class Astar:
    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 heuristic(self, node, goal, D=1, D2=2 ** 0.5):  # Diagonal Distance
        dx = abs(node.position[0] - goal.position[0])
        dy = abs(node.position[1] - goal.position[1])
        return D * (dx + dy)


    def aStar(self, maze, start, end):
        # startNode와 endNode 초기화
        startNode = Astar(None, start)
        endNode = Astar(None, end)

        # openList, closedList 초기화
        openList = []
        closedList = []

        # openList에 시작 노드 추가
        openList.append(startNode)

        # endNode를 찾을 때까지 실행
        while openList:

            # 현재 노드 지정
            currentNode = openList[0]
            currentIdx = 0

            # 이미 같은 노드가 openList에 있고, f 값이 더 크면
            # currentNode를 openList안에 있는 값으로 교체
            for index, item in enumerate(openList):
                if item.f < currentNode.f:
                    currentNode = item
                    currentIdx = index

            # openList에서 제거하고 closedList에 추가
            openList.pop(currentIdx)
            closedList.append(currentNode)

            # 현재 노드가 목적지면 current.position 추가하고
            # current의 부모로 이동
            if currentNode == endNode:
                path = []
                current = currentNode
                while current is not None:
                    # maze 길을 표시하려면 주석 해제
                    x, y = current.position
                    maze[x][y] = 2
                    path.append(current.position)
                    current = current.parent
                return path[::-1]  # reverse

            children = []
            # 인접한 xy좌표 전부
            for newPosition in [(0, -1), (0, 1), (-1, 0), (1, 0)]:

                # 노드 위치 업데이트
                nodePosition = (
                    currentNode.position[0] + newPosition[0],  # X
                    currentNode.position[1] + newPosition[1])  # Y
                    
                # 미로 maze index 범위 안에 있어야함
                within_range_criteria = [
                    nodePosition[0] > (len(maze) - 1),
                    nodePosition[0] < 0,
                    nodePosition[1] > (len(maze[len(maze) - 1]) - 1),
                    nodePosition[1] < 0,
                ]

                if any(within_range_criteria):  # 하나라도 true면 범위 밖임
                    continue

                # 장애물이 있으면 다른 위치 불러오기
                if maze[nodePosition[0]][nodePosition[1]] != 0:
                    continue

                new_node = Astar(currentNode, nodePosition)
                children.append(new_node)

            # 자식들 모두 loop
            for child in children:

                # 자식이 closedList에 있으면 continue
                if child in closedList:
                    continue

                # f, g, h값 업데이트
                child.g = currentNode.g + 1
                child.h = ((child.position[0] - endNode.position[0]) **
                        2) + ((child.position[1] - endNode.position[1]) ** 2)
                # child.h = heuristic(child, endNode) 다른 휴리스틱
                # print("position:", child.position) 거리 추정 값 보기
                # print("from child to goal:", child.h)
                
                child.f = child.g + child.h

                # 자식이 openList에 있으고, g값이 더 크면 continue
                if len([openNode for openNode in openList
                        if child == openNode and child.g > openNode.g]) > 0:
                    continue
                        
                openList.append(child)

    def run(self, maze, start, end):
        path = self.aStar(maze, start, end)
        return maze, path

일단 run 함수에 방금 만들었던 행렬을 매개변수로 넣어주고 start지점은 지금 로봇의 위치입니다.

행렬로 보았을 때 

로봇의 위치가 현재 12행, 7열에 위치하고 있습니다. run은 (11, 6)이 될 것이며 

end는 보내고 싶은 위치입니다. 위치는 4행 11열로 하겠습니다. (3, 10)

print("맵입니다.")
for i in my_map:
    print(i)
print()

temp_map = my_map
make_route = Astar()
start = (11, 6)
end = (3,10 )
result, path = make_route.run(temp_map , start, end)

print("경로 생성 맵입니다. 숫자 2는 경로입니다.")
for i in result:
    print(i)

2의 경로가 현재 로봇이 이동할 경로를 나타냅니다. 그러면 전역경로를 생성하는 과정은 모두 마무리가 되었습니다. 이제 남은 건 이를 ROS상에서 로봇이 작동하게 만드는 것입니다.