Major update - almost complete nav2