subscriber seems to be working, publisher is now failing
merge
Merge branch 'master' of github.com:bennypi/uarm-in-gazebo