Added waypoints_reached callback to nav.py
Changes and code cleanup in APF, still not perfect though.