Starting to implement waypoint