Adding an algorithm to fill routes with stops (every 500 meters)