Making robot navigate closest edge when not at node