WIP: Deadlock issue with new locking mechanism for setting waypoints