Fix waypoint path

Fixes #4 @jeremybenaim