Added benchmarking Methods

Implemented benchmark to test speed of waypoint functions.