Finished the lab, made almost everything work. Just need to make the check if the start position is on an obstacle
Removed unexplored grid to stop from murdering rviz on large maps with high resolution (ie, ak_220)