Planning a route using step by step search