First attempt to connect the original code with my own
Merge branch 'indigo-devel' of https://github.com/nestormh/rsgm_ros into indigo-devel