Merge pull request #2 from RoboGroup3/master

Updates from Marco
Merge pull request #3 from RoboGroup3/master

Updates from Marco
Adds a bagfile
Adds stops in sequence of movement
Changes it back to before
Fixes from robot
Merge pull request #11 from JohanJohansson/master

updates from Johan