Merge pull request #23 from tobiasandersson/master

Mapping progress
Merge pull request #24 from tobiasandersson/master

mapping progress
Merge pull request #25 from tobiasandersson/master

Mapping progress
fix robot's marker position and rotation
Merge pull request #26 from mlosch/master

fix robot's marker position and rotation
solved stopping bug