Added launch file for convenience.
Added goal and initial pose calculation. Pending: Astar search, heuristic, move cost functions