Waypoints

Added AGV waypoints for dijkstra implementation later