Simultaneous Localization And Mapping - And Path finding