RP-248 Migrate to ros_control

Remove custom gazebo plugin, fix #7