Add waypoints_reached topic and improve some code logics.