hw4_navplan.py