이전 글 "세선화 결과로부터 위상지도(topology map) 만들기에서 격자지도로부터 위상지도를 만들었습니다. 이제 위상지도에서 로봇의 출발지와 목적지가 정해지면 최적의 주행경로를 계산하는 방법 중 하나인 Dijkstra 알고리즘을 구현해 보겠습니다. Dijkstra 알고리즘에 대해서는 위키피디아 페이지를 참고하십시오: http://en.wikipedia.org/wiki/Dijkstra's_algorithm
아래는 구현된 소스코드입니다.
프로그램의 실행 결과는 다음과 같습니다.
