Completed Goal cost function, Navigation function need to be debug
Completed Goal cost function, Navigation function need to be debug