Having some problems with implementing OccupancyGridAlgorithm, probably waiting for some stuff from others