경로 계획기 NavFn / Smac

Global costmap이 "각 셀의 통행료(cost)"를 알려준다고 했습니다. 그렇다면 출발지에서 목적지까지 통행료 합이 가장 적은 길은 어떻게 찾을까요? 그 일을 하는 게 경로 계획기(planner) 입니다.

costmap을 그래프로 보면 최단 경로 문제다

costmap의 셀 하나하나를 노드로, 인접 셀을 간선으로 보면 — 이건 고전적인 최단 경로 그래프 탐색입니다. Nav2의 planner는 대표적으로 두 종류입니다.

• NavFn — Dijkstra/A* 계열. costmap 전체에 "potential field(전위장)"를 전파한 뒤, 목적지에서 역추적해 경로를 뽑습니다.
• Smac Planner — 격자(2D) 또는 Hybrid-A*(차량 운동학 고려). 회전 반경이 있는 로봇에 적합.

도식 렌더링 중…

직관은 이렇습니다 — 출발점에서 물결이 퍼지듯 cost가 누적되며 퍼져 나가고, 목적지에 닿으면 그 물결을 거슬러 내려오며 가장 싼 길을 따라옵니다. 벽(lethal 셀)은 물결이 못 넘으니 자연히 피해 갑니다.

이 파동 전파를 코드로 보면 Dijkstra 그 자체입니다.

# 출발에서 물결처럼 cost를 전파 (Dijkstra / NavFn)
frontier = [(0, start)]
while frontier:
    cost, cell = heappop(frontier)
    for nb in neighbors(cell):
        new = cost + costmap[nb]          # 셀 통행료 누적
        if new < dist[nb]:
            dist[nb] = new
            heappush(frontier, (new, nb))
# 목적지에서 dist를 거슬러 내려오면 최단 경로

계획기와 컨트롤러는 분업한다

여기서 중요한 구분: planner는 "어디로 갈지"(전역 경로)만 정합니다. 그 경로를 실제 바퀴 속도로 따라가는 건 다음 글의 컨트롤러(controller) 몫입니다.

도식 렌더링 중…

이 분업 덕분에 planner는 "긴 안목의 경로"에, controller는 "즉석 추종·회피"에 각자 집중합니다.

costmap이 목표를 못 덮으면 — Off Grid

planner의 가장 흔한 실패는 알고리즘이 아니라 costmap 범위 문제입니다.

⚠️ "Pose Goes Off Grid" — 목적지(또는 로봇)가 costmap이 덮는 영역 바깥에 있으면, planner는 자기가 모르는 좌표로의 경로를 만들 수 없어 실패합니다(failed to create plan). 환경이 양수 좌표 너머로 넓은데 costmap 범위를 작게 잡으면 이게 납니다.

💡 플러그인 이름 표기 — Nav2 버전에 따라 plugin 경로 표기(/ vs ::)가 다릅니다. 표기가 안 맞으면 planner가 아예 로드되지 않으니, 쓰는 배포판의 표기를 확인하세요.

즉, planner가 실패할 땐 알고리즘을 의심하기 전에 costmap이 출발·목적지를 모두 덮는지부터 확인하는 게 빠릅니다.

한 줄 정리

📌 Planner는 costmap을 그래프로 보고 최단 경로를 푸는 단계다(NavFn=Dijkstra/A*, Smac=격자/Hybrid-A*). 경로계획(planner)과 추종(controller)은 분업하며, planner 실패의 단골 원인은 알고리즘이 아니라 "목표가 costmap 밖(Off Grid)"이다.