added parking lot tests for basic waypoint nav.
updated waypoint tests
Updated parking lot tests
Partway through merge
Finished merging