Added code to display markers on the map. Currently does not work.
Default stopping distance: 5cm Distance to be kept from walls: 3cm.