Slam 65 - Make our stuff work again

[erickmu1]

• The task of ensuring it works on Caffeine physical robot will be addressed by issue #43
• Most of this is code re-structuring
• Does not use stereo cam features at this moment as they are not complete as well as don't pertain to the Navigation with Lidar project

NOTE: there are some changes that would still need to be made (eg. topic renaming) but at this point, I think it is at an appropriate level