Clean up nav2 code