Added a Map class to represent obstacles, integrated initial pathplanning, some minor fixes