Added path_planning in different structure