Lots of fixes for this package

I tested these changes along with slam_gmapping.