Improved the ROS interface
Removed redundant return
Merge branch 'develop'