Added magical stuff to expand the map
Fixed a few topic things, rebooting now to see if it fixes rviz being stupid