working version of base planning without collision check
Modified Astar