Hopefully fixed the areWeThereYet method. Testing on turtlebot