merged in latest changes from ros-planning repo
missed one conflict
Merge pull request #9 from KaijenHsiao/merge_ros_planning

Merged in latest changes to the nav stack (needed for clearing sonar layer)