waypoints are visualized now, closes #19