Merge pull request #1 from areverberi/slam

Pulling from slam, created branch from master instead of slam
Added all ROS-relevant code